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ABSTRACT 


Because  of  range  limitations  imposed  by  speed  and  power  supplies,  covert  launch 
and  recovery  of  Autonomous  Underwater  Vehicles  (AUVs)  near  the  operating  area  will  be 
required  for  their  use  in  many  military  applications.  This  report  contains  source  code 
implemented  on  the  Phoenix  AUV  in  support  of  recovery  in  a  small  stationary  tube. 

Implementation  involves  the  development  of  low-level  behaviors  for  sonar  and 
vehicle  control,  mid-level  tactics  for  recovery  planning,  and  a  mission-planning  system  for 
translating  high-level  goals  into  an  executable  mission.  Sonar  behaviors  consist  of  modes 
for  locating  and  tracking  objects,  while  vehicle  control  behaviors  include  the  ability  to 
drive  to  and  maintain  a  position  relative  to  a  tracked  object.  Finally,  a  mission-planning 
system  allowing  graphical  specification  of  mission  objectives  and  recovery  parameters  is 
implemented. 
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I.  INTRODUCTION 


This  technical  report  contains  P hoenix  Autonomous  Underwater  Vehicle  (AUV) 
source  code  that  was  developed  during  the  conduct  of  research  in  support  of  recovery  in  a 
small  tube  [Davis  96].  The  technical  report  is  divided  into  four  sections.  The  first  section 
consists  Underwater  Virtual  World  (UVW)  [Brutzman  94]  source  code  that  was  developed 
to  support  sonar  simulation  and  visualization.  The  second  section  consists  of  execution 
level  software  that  was  developed  to  provide  low  level  behaviors  such  as  sonar  target 
tracking  and  station  keeping.  The  third  section  consists  of  tactical  level  software  that 
implements  higher  level  tactics  for  recovery  path  planning  and  execution  level  command 
generation.  The  final  section  consists  of  source  code  for  a  mission  planning  expert  system 
that  was  developed  to  support  rapid  executable  mission  generation.  The  source  code 
included  in  this  technical  report  does  not  include  all  Phoenix  source  code,  but  rather  only 
that  code  that  was  modified  or  developed  during  the  conduct  of  this  research.  This  code  is 
compiled  and  linked  with  existing  Phoenix  software  to  implement  the  Rational  Behavior 
Model  software  architecture  [Byrnes  92] .  All  source  code  contained  in  this  technical  report 
is  available  onUne  individually  or  as  part  of  the  Phoenix  AUV  software  package  in  .tar 
format  at 

http. II WWW. stl.nps. navy. mil! '^brutzman! dissertation! software  reference.html 
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IL  UNDERWATER  VIRTUAL  WORLD  SOURCE  CODE 

This  section  contains  source  code  for  the  UVW.  Included  code  is  only  that  code 
which  was  written  or  modified  during  the  conduct  of  this  thesis.  The  file  viewer.C  must  be 
compiled  and  linked  with  the  file  AUVglobals.H  to  make  the  viewer  portion  of  the  UVW. 
The  file  ivSonar.C  must  be  compiled  and  linked  with  the  files  dynamics.C,  UUVBody.C, 
AUVsocket.C,  DISNetworkedRigidBody.C,  RigidBody.C,  Hmatrix.C,  Quatemion.C, 
VectorSd.C,  AUVglobals.H,  UUVmodeLH  and  AUVmodel.H  to  make  the  dynamics 
module  of  the  UVW.  These  files  and  installation  instructions  and  examples  are  available 
online  at 

http:llwww.stl.nps. navy. mill '^brutzman! dissertation! software _r^erence.html 
Files  are  available  individually  or  as  a  group  in  a  .tar  package. 
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////// 


Program: 
Description: 

Author: 
Revised: 
System: 
Compiler: 
Compilation: 
References 


Dissertation: 


Advisors : 
Notes : 


Future  work: 


vi ewer . C 

Openinventor  viewer  for 
NPS  AUV  Underwater  Virtual  World 

Don  Brutzman,  Duane  Davis 

25  June  96 

Irix  5.3 

ANSI  C++  /  Openinventor  API 
irix>  make  viewer 

JfS  199?  Florida, 

(2)  Macedonia,  Michael,  ZeswiCz,  Steven,  and  Locke  John 

Sersion^Z^O  3^^wfvai'''r  (DIS)  multicast 

version  2.0.3,  Naval  Postgraduate  School,  February  94. 

(3)  Zeswitz  Steven,  ''NPSNET:  Integration  of  Distributed 

Architectiire^anrt^T'^f°'^  Protocol  for  Communication 

Architecture  and  Information  Interchange."  master' c- 

Naval  Postgraduate  School,  Monterey  California,  28  May  1993 . 

F.,  Software  Reference:  A  Virtual  World 

l^PS  cs  Oio°Pr°Nf  U^^^rwater  Vehicle,  technical  repo!?^ 

NPS  CS  010  94,  Naval  Postgraduate  School,  Monterey^ 
California,  December  1994.  The  accompanying  public 

coS  iLludes  source 

f tp : / / taurus . cs . nps . navy . mil /pub/auv/auv_uvw . h tml 
Dr.  Mike  Zyda,  Dr.  Bob  McGhee  and  Dr.  Tony  Healey 
totoater  virtual  world  is  in  feet,  standard  DIS  PDU  is 

Inventor  1.0.1  and  DIS  libraries  were  NOT  comoatibl^s  rina 

DIS^^ibra^ieI^confr^^®^®  triggered  by  malloL  in  thf 
ReLvtn^  nxc  With  the  Inventor  window. 

Automated  camera  control  fixed. 

.IV  file  load  via  WWW  URLs  passed  by  DIS  Message  PDUs 


Message  PDUs  including  sound/speech. 

Add  CAMERA_FROM_SONAR  and  camera  selection  menu. 

Get  full  scene  optimization  by  ivquick  fixed. 

Ocean  current  and  DiveTracker  range  visualization. 
Optimization. 

Keyboard  or  button  interactivity  (using  SceneViewer?) 
ivf ix  gives  bad  output  ? ! 


*/ 

///////////////////////////////////////////////////////////////////////////// 

//  from  relnotes  inventor_dev  4  (version  2.1) 
tinclude  <inttypes.h> 


#include  . /dynamics /AUVglobals . H" 


#include 
#include 
# include 
#include 
#include 
#include 
#include 
#include 
#include 
^include 
#include 


<iostream.h> 

<f stream.h> 

<iomanip.h>  //  must  follow  iostream.h 
<string.h> 

<math.h> 

<time.h> 

<getopt .h> 

<stdlib.h> 

<netdb.h> 

<netinet/in.h> 

<sys / types .h> 


///////////////////////////////////////////////////////////////////////////// 

/// 

# 

//  DIS  includes.  See  Makefile  for  other  DIS  #include  files;  they  must  match. 

#include  ". . /DIS .mcast/h/disdef s .h" 

#include  ". . /DIS.mcast/h/appearance.h" 

extern  "C"  {  printPDU  (char  *) ;  };  If  function  prototype  provided  for 

//  compatibility,  missing  from  DIS  library 

///////////////////////////////////////////////////////////////////////////// 

/// 


#include  <X11/Intrinsic .h> 
#includG  <Xll/keysym.h> 
tinclude  <Xm/Xm.h> 

#include  <Xm/CascadeBG.h> 
#include  <Xm/Fonn.h> 
#include  <Xm/RowColumn.h> 
#include  <Xm/PushB.h> 
#include  <Xm/PushBG -h> 

# include  <Xm/Separator .h> 
#include  <Xm/S€paratoG.h> 
#include  <Xm/ToggleB.h> 
#include  <Xm/ToggleBG .h> 


#include 

#include 

#include 

#include 

#include 


<Inventor/SbBasic . h> 
<Inventor/SbVi€wportRegion.h> 
<Inventor/So .h> 

<Inventor/SoDB .h> 

<Inventor /So Input . h> 
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#include  <Inventor/SoPickedPoint .h> 

^include  <Inventor/Xt/SoXt .h> 

#include  <Inventor/Xt/SoXtPrintDialog.h>  //  see  SoOff screenRender 
#include  <Inventor/Xt /SoXtRenderArea . h> 

#include  <Inventor/Xt/SoXtMaterialEditor.h> 

#include  <Inventor/Xt/viewers/SoXtExaminerViewer .h> 
tinclude  <Inventor/Xt /viewers /SoXtFlyVi ewer. h> 

#include  <Inventor/Xt/viewers/SoXtPlaneViewer *h> 

#include  <Inventor/Xt/viewers/SoXtWalkViewer . h> 

#include  <Inventor/actions/SoAction.h> 

#include  <Inventor/actions/SoCallbackAction . h> 

#include  <Inv€ntor/actions/SoGLRenderAction .h> 

#include  <Inventor/actions/SoPickAction .h> 

#include  <Inventor/actions/SoRayPickAction.h> 
tinclude  <Inventor/actions/SoWriteAction. h> 

tinclude  <Inventor/engines/SoCalculator .h> 
tinclude  <Inven tor/ engines / SoEl apsedT ime. h> 
tinclude  <Inventor/engines/SoTiineCounter .h> 

tinclude  <Inventor/events/SoEvent .h> 
tinclude  <Inventor/events/SoKeyboardEvent .h> 

tinclude  <Inventor/nodes/SoConiplexity  .h> 
tinclude  <Inventor/nodes/SoCone.h> 
tinclude  <Inventor/nodes/SoCoordinate3 .h> 
tinclude  <Inventor/nodes/SoCube.h> 
tinclude  <Inventor/nodes/SoCylinder .h> 
tinclude  <Inventor/nodes/SoCone .h> 
tinclude  <Inventor/nodes/SoDirectionalLight .h> 
tinclude  <Inventor/nodes/SoDrawStyle .h> 
tinclude  <Inventor/nodes/SoEventCallback.h> 
tinclude  <Inventor/nodes/SoFaceSet . h> 
tinclude  <Inventor /nodes /SoGroup.h> 
tinclude  <Inventor/nodes/SoMaterial .h> 
tinclude  <Inventor/nodes/SoNurbsSurface.h> 
tinclude  <Inventor/nodes/SoPerspectiveCainera. h> 
tinclude  <Inv€ntor/nodes/SoPickStyle.h> 
tinclude  <Inventor/nodes/SoRotationXYZ. h> 
tinclude  <Inventor/nodes/SoScale. h> 
tinclude  <Inventor/nodes/SoSelection . h> 
tinclude  <Inventor/nodes/SoSeparator .h> 
tinclude  <Inventor/nodes/SoSphere . h> 
tinclude  <Inventor/nodes/SoSwitch .h> 
tinclude  <Inventor/nodes/SoTransf orm . h> 
tinclude  <Inventor/nodes/SoTransf ormSeparator . h> 
tinclude  <Inventor/nodes/SoTranslation . h> 

tinclude  <Inventor/nodes/SoTriangleStripSet .h>  //  order  matters  here 
tinclude  <Inventor/nodes/SoUnits . h> 

tinclude  <Inventor/sensors/SoTimerSensor .h> 


///////////////////////////////////////////////////////////////////////////// 


//  function 

prototypes 

static  int 

DIS_net_open 

{)  ; 

static  void 

DIS_net_close 

0  ; 

static  void 

DIS_Redraw_Callback 

(  void 

*  unused_data. 

SoSensor  *  unused__calling_sensor  ); 

double  auv_ST72 Strange  (  double  angle,  double  power,  double  range, 
SbViewport Region  *  viewport  ) ; 
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void  readCircles  (); 

///////////////////////////////////////////////////////////////////////////// 

#definG  PI  3.1415926535897932 

#dGf  ine  METERS__PER_FT  0.3048 
#dGfinG  FT_PER_METERS  3.2808 

int  DEADRECKON  =  TRUE ; 

int  READCIRCLEFILE  =  FALSE; 

ifstream  circlaFilG; 

//  utility  function  prototypes 

void  swap_float  (float  a,  float  b) ; 

double  sign  (double  x) ; 

double  degrees  (double  x) ;  //  radians  input 

double  radians  (double  x) ;  //  degrees  input 

double  arcclamp  (double  x) ; 

double  dnormalize  (double  angle^radians) ;  //  returns  0..2PI 

int  inormalize  (double  angle_radians)  //  returns  0..359 

{return  (int)  (degrees  (angle_radians)  +0.5)  %  360;} 

SoSeparator  *  readFile (const  char  *filenaine);  //  Inventor  Mentor  p.  284 

void  initialize_globals  (); 

void  parse_command_line_flags  (int  arge,  char  **  argv); 

///////////////////////////////////////////////////////////////////////////// 
//  global  data  (referenced  in  callback  routines,  thus  defined  here) 

#ifndef  VIEWER_GLOBALS 

#define  VIEWER_GLOBALS  //  prevent  warnings  if  multiple  #includGS  present 
SoSeparator  *  root; 

SoSeparator  *  scenery  =  new  SoSeparator; 

//  permit  multiple  loads  from  command  line 
SoTransform  *  AUV_posi tion_node; 

//  initializers  needed  to  avoid  startup  segmentation  fault 

SoRotationXYZ  *  rotate_AUV_z  =  new  SoRotationXYZ; 

SoRotationXYZ  *  rotate_AUV_y  =  new  SoRotationXYZ; 

SoRotationXYZ  *  rotate_AUV_x  =  new  SoRotationXYZ; 

SoRotationXYZ  *  rotate_AUV_bow_rudders  =  new  SoRotationXYZ; 

SoRotationXYZ  *  rotate_AUV_stern_rudders  =  new  SoRotationXYZ ; 

SoRotationXYZ  *  rota te__AUV_bow__p lanes  =  new  SoRotationXYZ; 

SoRotationXYZ  *  rotate_AUV__stern_p lanes  =  new  SoRotationXYZ ; 

SoCone  *  thrusterWakeFV  =  new  SoCone; 

SoCone  *  thrusterWakeAV  =  new  SoCone; 

SoCone  *  thrusterWakeFH  =  new  SoCone; 

SoCone  *  thrusterWakeAH  =  new  SoCone; 

SoCone  *  conePropellerWakeStbd  =  new  SoCone; 

SoCone  *  conePropellerWakePort  =  new  SoCone; 

SoSwitch  *  whichWakeFV  =  new  SoSwitch; 

SoSwitch  *  whichWakeAV  =  new  SoSwitch; 
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SoSwitch 

SoSwitch 


*  whichWakeFH 

*  whichWakeAH 


new  SoSwitch; 
new  SoSwitch; 


SoTransf orm 
SoTransform 
SoTransfonn 
SoTransform 
SoTransform 
SoTransform 
SoTransform 
SoTransform 


*  topsideBow 

*  topsideStern 

*  bottoinsideBow 

*  bottomsideStern 

*  leftsideBow 

*  leftsideStern 

*  rightsideBow 

*  rightsideStern 


=  new  SoTransform; 
=  new  SoTransform; 
=  new  SoTransform; 
=  new  SoTransform; 
=  new  SoTransform; 
=  new  SoTransform; 
=  new  SoTransform; 
=  new  SoTransform; 


SoTransform 

SoTransform 

SoRotationXYZ 

SoCone 


*  xfConeSonarSTlOOO 

*  offsetSTlOOO 

*  rotConeSonarSTlOOO 

*  coneSonarSTlOOO 


=  new  SoTransform; 

=  new  SoTransform; 

-  new  SoRotationXYZ; 
=  new  SoCone; 


SoTransform 

SoTransform 

SoRotationXYZ 

SoScale 

SoCone 


*  xfConeSonarST725 

*  offsetST725 

*  rotConeSonarST725 

*  scaleConeSonarST725 

*  coneSonarST725 


=  new  SoTransform; 

=  new  SoTransform; 

=  new  SoRotationXYZ; 
=  new  SoScale; 

=  new  SoCone; 


SoMaterial  * 
SoMaterial  * 
SoMaterial  * 
SoMaterial  * 
SoMaterial  * 
SoMaterial  * 
SoMaterial  * 
SoMaterial  * 
SoMaterial  * 
SoMaterial  * 
SoMaterial  * 


silver  =  new  SoMaterial; 
gold  =  new  SoMaterial; 

brass  =  new  SoMaterial; 
chrome  =  new  SoMaterial; 
npsblue  =  new  SoMaterial; 
seagreen  =  new  SoMaterial; 
seasurface=  new  SoMaterial; 
seacolor  =  new  SoMaterial; 
darkgreen  =  new  SoMaterial; 
sonar_cone_color725  =  new  SoMaterial; 
sonar_cone_colorl000  ~  new  SoMaterial; 


SoDrawStyle  *  wires  =  new  SoDrawStyle; 


SoScale  *  MetersToFeet  =  new  SoScale; 
SoScale  *  InchesToFeet  =  new  SoScale; 
SoScale  *  FeetToInches  =  new  SoScale; 


SoTransform  *  MineFieldTransform  =  new  SoTransform; 
SoScale  *  MineFieldScale  =  new  SoScale; 

SbViewportRegion  *  myRegion; 


#define  CAMERA^FREE  0 
#define  CAMERA_TO_AUV  1 
#define  CAMERA_FROM_AUV  2 
#define  CAMERA_FROM_SONAR  3 


static 

int 

whichCamera 

static 

int 

PRINTDIALOG 

static 

int 

BACKGROUNDCOLORDIALOG 

static 

int 

TEXTURE 

static 

int 

SCREENDOOR 

static 

int 

LIGHTSOUT 

static 

int 

MINEFIELD 

=  CAMERA_TO_AUV;  //  default  camera 
=  FALSE; 

=  FALSE; 

=  FALSE; 

=  FALSE; 

=  FALSE; 

=  FALSE; 


SoPerspectiveCamera  *  PerspectiveCameraFree  =  new  SoPerspectiveCamera ; 
SoPerspec ti veCamera  *  PerspectiveCameraToAUV  =  new  SoPerspectiveCamera; 
SoPerspectiveCamera  *  Perspecti veCamera FromAUV  =  new  SoPerspectiveCamera; 
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SoKeyboardEvent 


key _e vent 


=  new  SoKeyboardEvent; 


SbVec3f  behindAUVPosition; 

SbVec3f  aheadOfAUVPosition; 

SbVec3f  priorAUVPosition; 

SbVec3f  currentAUVPosition; 

SbVec3f  behindSonarPosition; 

SbVec3f  aheadOfSonarPosition; 

SbVec3f  priorCameraPosition; 

SbVec3f  currentCameraPosition; 

SbVec3f  priorCameraOf fset; 

SbVec3f  cur rent Came raOff set; 

SbVec3f  orientationRotationAxis; 
float  orientationRotationAngle; 

SbVec3f  standardCameraOf f set  =  SbVec3f  (  3.0,  1.0,  6.0  ); 

SbVec3f  AUVCameraOf fset  =  SbVec3f  (  0.0,  1.2,  0.0  ),- 

SbVec3f  AUVCameraPointTo  =  SbVec3f  (  +3.0,  0.0,  0.0  )  ,- 

float  standardCameraFocalDistance  =  20.0; 


SoSeparator  *  coxnmand_line_node; 


static  docket 

static  double 

static  int 

static  int 

char 

char 

static  SoUnits  * 

//  static  struct  in__addr 

1 1  static  struct  hostent 

double  ST1000Complexity_of f 
double  STlOOOComplexi ty_on 
double  ST725Complexi ty_of f 
double  ST725Complexity_on 


t  ime_s  tamp_of_current_PDU, 
t  ime_of  _PDU_receipt , 
current_clock, 
del ta_c lock ; 
delta_time; 

PDU_overdue  ; 

DIS_port_open; 

port  [  6]; 
group  [30]; 

units feet; 

inaddr; 

*  hp; 

=  0.05; 

=  0.2; 

=  0.1; 

=  0.4; 


SoComplexity  *  wakeComplexi ty  =  new  SoComplexity ; 

SoComplexity  *  STlOOOComplexity  =  new  SoComplexity; 

SoComplexity  *  ST725Complexity  =  new  SoComplexity; 

///////////////////////////////////////////////////////////////////////////// 
//  all  NPS  AUV  dimensions  here  in  inches 


#def ine 

HULLBODYLENGTH 

54.00 

#define 

HULLBODYWIDTH 

16.50 

tdefine 

HULLBODYHEIGHT 

10.0 

#def ine 

SEAM 

-27.00 

// 

#def ine 

STERN 

-43.50 

If 

#def ine 

LEFT 

8.25 

// 

-  HULLBODYLENGTH  /  2.0 

-  HULLBODYLENGTH  /  2.0 
HULLBODYWIDTH  /  2 . 0 


16.5 
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♦define  RIGHT  -8.25  //  -  HULLBODYWIDTH  /  2.0 

♦define  TOP  5.00  //  HULLBODYHEIGHT  /  2.0 

♦define  BOTTOM  -5.00  II  -  HULLBODYHEIGHT  /  2.0 

♦define  DOMECONTROLPT  25.00  //  nosecone  NURBS  surface  cooordinates 

♦define  DOMECONTROLPTHALF  20.00  //  offsets  to  help  define  shape 

♦define  TOPHALF  4.75 

♦define  BOTTOMHALF  -4.75 

♦define  LEFT_HALF  8.00 

♦define  RIGHTHALF  -8.00 

♦define  CENTER  0.00 

♦define  FINLENGTH  6.00  //  fins  need  to  be  tapered 

♦define  FINWIDTH  0.75  //  fin  thickness  1"  at  bottom,  0.5"  at  top 

♦define  FINHEIGHT  6.75 

♦de  f i ne  F INOFFSETFORWARD  24.0 

♦define  FINOFFSETAFT  -33.0 

♦define  FINOFFSETLEFT  12.10  //  (HULLBODYWIDTH  /  2)  +  (FINHEIGHT  /  2)  +  .5" 

♦define  FINOFFSETRIGHT  -12.10 

♦define  FINOFFSETUP  8.875  //  (HULLBODYHEIGHT  /  2)  +  (FINHEIGHT  /  2)  +  .5" 

♦define  FINOFFSETDOWN  -8.875 

♦define  THRUSTERID  3.0 

♦define  THRUSTEROD  3.5 

♦define  THRUSTERFORWARDV  13.0  //  forward  SEAM  -  14 

♦define  THRUSTERAFTV  -  21.0  //  SEAM  +  6 

♦define  THRUSTERFORWARDH  19.0  //  forward  SEAM  -  8 

♦define  THRUSTERAFTH  -  26.0  //  SEAM  +  1 

♦define  SHAFTOFFSETLEFT  3.75 

♦define  SHAFTOFFSETRIGHT  -3.75 

//  reverify  these  dimensions  after  rebuild 

♦define  CARDCAGELEFT  4 . 00 

♦define  CARDCAGER I GHT  -4.00 

♦define  CARDCAGELENGTH  12.00 

♦define  CARDCAGEWIDTH  7.00 

♦define  CARDCAGEHEIGHT  8.00 


///////////////////////////////////////////////////////////////////////////// 
♦endif  //  VIEWER_GLOBALS 

///////////////////////////////////////////////////////////////////////////// 
SoSeparator  *  makeAUV  () 

{ 

rotate_AUV_z->angle. setValue  (  AUV_psi) ; 
rotate_AUV_z->  axis . setValue  (SoRotationXYZ: :Z) ; 

rota te_AUV_y->angle. setValue  (-  AUV_theta) ; 
rotate_AUV_y->  axis. setValue  (SoRotationXYZ::Y); 

rotate_AUV_x->angle. setValue  (  AUV_phi) ; 
rotate_AUV_x->  axis . setValue  (SoRotationXYZ::X); 

///////////////////////////////////////////////////////////////////////////// 
//  NPS  AUV  hull  body  center  is  at  [0.0  0.0  0.0], 

//  volumetric  center  of  buoyancy 

//  fin  transformations  forward 

SoTransform  *xfl  =  new  SoTransform; 

xfl->translation.setValue(  FINOFFSETFORWARD,  0.0,  FINOFFSETUP); 

SoTransform  *xf2  =  new  SoTransform; 

xf2->translation. setValue (  0.0,  0.0,  FINOFFSETDOWN  -  FINOFFSETUP); 
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SoTransform  *xf3  =  new  SoTransfom; 

xf3->translation.setValue(  FINOFFSETFORWARD,  FINOFFSETLEFT,  0.0); 
SoTransform  *xf4  =  new  SoTransform; 

xf4->translation.setValue(  0.0,  FINOFFSETRIGHT  -  FINOFFSETLEFT,  0.0); 
//  fin  transformations  aft 
SoTransform  *xf5  =  new  SoTransform; 

xf5->translation.setValue(  FINOFFSETAFT,  0.0,  FINOFFSETUP) ; 
SoTransform  *xf6  =  new  SoTransform; 

xf6->translation,setValue(  0.0  ,  0.0,  FINOFFSETDOWN  -  FINOFFSETUP); 
SoTransform  *xf7  =  new  SoTransform; 

xf7->translation.setValue(  FINOFFSETAFT,  FINOFFSETLEFT,  0.0); 
SoTransform  *xf8  =  new  SoTransform; 

xf8->translation.setValue{  0.0,  FINOFFSETRIGHT  -  FINOFFSETLEFT,  0.0); 

//  90  degree  increment  rotations  -  get  #define'd  PI  values 

SoRotationXYZ  *ro90x  =  new  SoRotationXYZ; 
ro90X“‘>angle.setValue  (3,141592653  /  2.0); 
ro90x->  axis  .setValue  (SoRotationXYZ  :  :X)  ; 

SoRotationXYZ  *ro90y  =  new  SoRotationXYZ; 
ro90y->angle. setValue  (3.141592653  /  2.0); 
ro90y->  axis . setValue  (SoRotationXYZ : :Y) ; 

SoRotationXYZ  *ro90z  =  new  SoRotationXYZ; 
ro90z->angle, setValue  (3.141592653  /  2.0); 
ro90z->  axis .setValue  (SoRotationXYZ :: Z) ; 

SoRotationXYZ  *rol80x  =  new  SoRotationXYZ; 
rol80x->angle. setValue  (3.141592653); 
rol80x->  axis. setValue  (SoRotationXYZ: :X) ; 

SoRotationXYZ  *rol80y  =  new  SoRotationXYZ; 
rol80y->angle. setValue  (3 .141592653) ; 
rol80y->  axis . setValue  (SoRotationXYZ::Y); 

SoRotationXYZ  *rol80z  =  new  SoRotationXYZ; 
rol80z->angle. setValue  (3.141592653); 
rol80z->  axis . setValue  (SoRotationXYZ: :Z) ; 

SoRotationXYZ  *ro270x  =  new  SoRotationXYZ; 
ro270x~>angle. setValue  (-  3.141592653  /  2.0); 
ro270x-">  axis. setValue  (SoRotationXYZ :  :X)  ; 

SoRotationXYZ  *ro270y  =  new  SoRotationXYZ; 
ro270y->angl€. setValue  (-  3.141592653  /  2.0); 
ro270y->  axis . setValue  (SoRotationXYZ : :Y) ; 

SoRotationXYZ  *ro270z  =  new  SoRotationXYZ; 
ro270z->angle. setValue  (-  3.141592653  /  2.0); 
ro270z->  axis. setValue  (SoRotationXYZ :: Z) ; 

//  construct  NPS  AUV  hull  body 
SoCube  *hull  =  new  SoCube; 
hull->width  =  HULLBODYLENGTH; 
hull->height  =  HULLBODYWIDTH; 
hull->depth  =  HULLBODYHEIGHT; 

//  construct  a  control  fin 
SoCube  *fin  =  new  SoCube; 
fin~>width  =  FINLENGTH; 
fin->height  =  FINWIDTH; 
fin->depth  =  FINHEIGHT; 
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//  construct  fin  pairs 


rotate_AUV_bow_rudders  =  new  SoRotationXYZ; 
rotate_AUV_bow_rudders~>axis .setValue  (SoRotationXYZ : : Z) ; 
rotate_AUV_bow_rudders->angle. setValue  (  0.0  ); 

rotate_AUV_stern_rudders  =  new  SoRotationXYZ; 
rotate_AUV_stern_rudders~>axis.setValue  (SoRotationXYZ: :Z) ; 
rotate_AUV_stern_rudders->angle. setValue  (  0.0  ); 

rotate_AUV_bow_planes  =  new  SoRotationXYZ; 
rotate_AUV_bow_planes~>axis.setValuG  (SoRotationXYZ: :Z) ; 
rotate_AUV_bow_planeS“>angle.setValuG  (  0.0  ); 

rotate_AUV_stGrn__planes  =  new  SoRotationXYZ; 
rotate_AUV__stern_planes->axis.setValue  (SoRotationXYZ: :Z) ; 
rotate_AUV_stern_j)lanes->angle.setValue  (  0.0  ); 

//  construct  forward  vertical  fins  (bow  rudders) 

SoSeparator  *fvfins  =  new  SoSeparator; 
fvfins->addChild(  xfl  ); 
fvfins“>addchild(  rotate_AUV_bow_rudders  ); 
fvfins->addChild(  fin  ); 
fvfins“>addChild{  xf2  ); 

fvfins->addChild(  rolSOx  );  //  net  rotation  180 

fvfinS“>addChild(  fin  ); 

//  construct  aft  vertical  fins  (stern  rudders) 

SoSeparator  *avfins  =  new  SoSeparator; 
avfins->addChild(  xf5  ); 

avfins->addChild(  rotate_AUV_stern_rudders  ); 
avfins->addChild(  fin  ); 
avfins->addChild(  xf6  ); 

avfins->addChild{  rolSOx  );  //  net  rotation  180 

avfins->addChild{  fin  ); 

//  construct  forward  horizontal  fins  (bow  planes) 

SoSeparator  *fhfins  =  new  SoSeparator; 
fhfins->addChild(  xf3  ); 

fhfins->addChild(  ro90x  );  //  net  rotation  90 

fhfins->addChild(  rotate_AUV_bow_planes  ); 
fhfins->addChild{  fin  ); 
fhfins~>addChild(  ro270x  ); 
fhfinS“>addChild(  xf4  ); 

fhfins->addChild(  ro270x  );  //  net  rotation  270  (in  case  of  fin  asyirnnetry) 
fhfins->addChild(  fin  ); 

//  construct  aft  horizontal  fins  (stern  planes) 

SoSeparator  *ahfins  =  new  SoSeparator; 
ahfins->addChild(  xf7  ); 

ahfins->addChild(  ro90x  );  //  net  rotation  90 

ahfins~>addChild(  rotate_AUV_stern_p lanes  ); 
ahfins->addChild(  fin  ); 
ahfins->addChild(  ro270x  ); 
ahfins->addChild(  xf8  ); 

ahfins->addChild(  ro270x  );  //  net  rotation  270  (in  case  of  fin  asymmetry) 

ahfinS“>addChild(  fin  ); 

//  construct  cylinders  to  represent  the  thrusters 
SoTransform  *xfl3  =  new  SoTransform; 

xfl3~>translation.SGtValue(THRUSTERFORWARDV,  0.0,  0.0} ; 

SoTransform  *xfl4  =  new  SoTransform; 
xf 14->translation . setValue (THRUSTERAFTV,  0.0,  0.0); 

SoTransform  *xfl5  =  new  SoTransform; 
xfl5“>translation. setValue (THRUSTERFORWARDH,  0.0,  0.0) ; 

SoTransform  *xfl6  =  new  SoTransform; 

xf 16 ~>translation. setValue (THRUSTERAFTH,  0.0,  0.0) ; 
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SoCy Under  *  tubeV  =  new  SoCylinder; 
tubeV->radius  =  THRUSTERID; 
tubeV>>height  =  HULLBODYHEIGHT  +  0.2; 

SoCylinder  *  tubeH  =  new  SoCylinder; 
tubeH->radius  =  THRUSTERID; 
tubeH->height  =  HULLBODYWIDTH  +  0.2; 

//  still  inches 

topsideBow->  translation . setValue (  0,  TOPHALF  +  AUV_bow_vertical ,  0); 
bottoinsideBow->  translation. setValue (  0,  BOTTOMHALF  +  AUV_bow_vertical ,  0); 
bottoinsideBow->  rotation . setValue (SbVecB f  {  0.0,  1.0,  0.0  ) ,  M_PI  ); 

topsideStern->  translation .setValue {0, TOPHALF  +  AUV_stern_vertical,0}; 
bottoinsideStern->translation. setValue (0, BOTTOMHALF  +  AUV_stern_vertical , 0 ) ; 
bottoinsideStern->  rotation. setValue {SbVecS f  {  0.0,  1.0,  0.0),  M_PI  ); 

leftsideBow->  translation. setValue {  0,  LEFT__HALF  +  AUV_bow_lateral ,  0); 
lef tsideBow->  rotation.setValue (SbVecSf  {  0.0,  1.0,  0.0  ),  M_PI  ); 

rightsideBoW’->  translation . setValue {  0,  RIGHTHALF  +  AUV_bow_lateral,  0); 

leftsideStern->  translation.setValue (0,  LEFT_HALF  +  AUV_stern_lateral,  0); 
leftsideStern->  rotation. setValue (SbVecSf  (  0.0,  1.0,  0.0  ),  M_PI  ); 

rightsideStern->  translation. setValue (0,  RIGHTHALF  +  AUV_stern_lateral,  0); 

thrusterWakeFV  =  new  SoCone;  //  global  for  callbacks 
thrusterWakeFV->height  =  AUV_bow_vertical  *  2.0; 

thrusterWakeFV->bottoinRadius  =  AUV_bow_vertical  /  4.0; 

thrusterWakeFV->parts  =  SoCone: : SIDES; 

SoSeparator  *  thriisterFV  =  new  SoSeparator; 

thrusterFV”>addChild{  xfl3  ); 
thrusterFV“>addChild(  ro90x  ); 
thrusterFV->addChild(  tubeV  ),* 
thrusterFV->addChild(  wires  ); 
thrusterFV->addChild {  wake Complexity  ); 
thrusterFV->addChild(  seagreen  ); 
thrusterFV->addChild(  whichWakeFV  ); 

thrusterFV->addChild {  thrusterWakeFV  ); 

thrusterWakeAV  =  new  SoCone;  //  global  for  callbacks 
thrusterWakeAV->height  =  AUV_stern_vertical  *  2.0; 

thrusterWakeAy->bottomRadius  =  AUV_stern_vertical  /  4.0; 
thrusterWakeAV->parts  =  SoCone :: SIDES ; 

SoSeparator  *  thrusterAV  =  new  SoSeparator; 
thrusterAV->addChild(  xfl4  ); 
thrusterAV“>addChild(  ro90x  ); 
thrusterAV->addChild  (  tubeV  ).; 
thrusterAV“>addChild(  wires  ); 
thrusterAV“>addChild(  wakeComplexity  ); 
thnisterAV->addChild{  seagreen  ); 
thrusterAV->addChild(  whichWakeAV  ); 
thrusterAV->addChild(  thrusterWakeAV  ); 

thrust erWakeFH  =  new  SoCone;  //  global  for  callbacks 
thrusterWakeFH->height  =  AUV_bow_lateral  *  2.0; 

thrusterWakeFH->bottoinRadius  =  AUV_bow_lateral  /  4.0; 
thrusterWakeFH->parts  =  SoCone; iSIDES; 

SoSeparator  *  thrusterFH  =  new  SoSeparator; 
thrusterFH->addChild(  xfl5  ); 
thrusterFH->addChild(  ro90y  ); 
thrusterFH->addChild(  tubeH  ); 
thrusterFH->addChild(  rolSOz  ); 
thrusterFH->addChild(  wires  ); 
thrusterFH“>addChild{  wakeComplexity  ); 
thrusterFH->addChild(  seagreen  ); 
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thrusterFH->addChild{  whichWalceFH  ); 
thrusterFH*‘>addChild(  thrusterWakeFH  ); 

thrusterWakeAH  =  new  SoCone;  //  global  for  callbacks 
l:hrusterWakeAH->height  =  AUV_stern_lateral  *  2*0; 

thrusterWakeAH-->bottoinRadius  =  AUV_stern_lateral  /  4.0; 
thrusterWakeAH->parts  =  SoCone: ; SIDES; 


SoSeparator  *  thrusterAH  =  new  SoSeparator; 


thrusterAH->addChild( 
thrusterAH“>addChi Id ( 
thrusterAH-'>addChild  { 
thrusterAH->addChild ( 
thrusterAH“>addChild ( 
thrusterAH“>addChild ( 
thrusterAH->addChild ( 
thrusterAH->addChild ( 
thrusterAH->addChild { 

//  construct  internal 


xfl6  ) ; 
ro90y  ) ; 
tubeH  ) ; 
rolSOz  ) ; 
wires  ) ; 

wakeComplexity  ) ; 
seagreen  ) ; 
whichWakeAH  )  ; 
thrusterWakeAH  ) ; 

CARDCAGEs  left  and  right 


SoCube  *cardcagelef tbox  =  new  SoCube; 
c  a  r dc  ageleftbox->width  =  CARDCAGELENGTH ; 
cardcageleftbox->height  =  CARDCAGEWIDTH; 
cardcageleftbox->depth  =  CARDCAGEH EIGHT; 


SoTransform  *xfcardcagelef t  =  new  SoTransform; 
xfcardcageleft->translation.setValue(  0.0,  CARDCAGELEFT,  0.0  ); 

SoSeparator  *cardcagelef t  =  new  SoSeparator; 
cardcageleft->addChild(  xf cardcagelef t  ); 
cardcageleft->addChild(  cardcagelef tbox  ); 

SoCube  *cardcagerightbox  =  new  SoCube; 
cardcagerightbox->width  =  CARDCAGELENGTH; 
cardcagerightbox->height  =  CARDCAGEWIDTH; 
cardcagerightbox->depth  =  CARDCAGEHEIGHT; 

SoTransform  *xfcardcageright  =  new  SoTransform; 
xf cardcageright->translation . setValue (  0.0,  CARDCAGERIGHT,  0.0  ) ; 

SoSeparator  *cardcageright  =  new  SoSeparator; 
cardcageright“>addChild(  xfcardcageright  ); 
cardcageright->addChild(  cardcagerightbox  ); 


///////////////////////.////////////////////////////////////////////////////// 
II  construct  main  body  out  of  parts 
SoGroup  *body  =  new  SoGroup; 

body~>addChild{  gold  ); 
body->addChild(  fvfins  ); 
body->addChild(  avfins  ); 
body“>addChild(  fhfins  ); 
bo(^“>addChild(  ahfins  ); 

body->addChild(  npsblue  ); 
body->addChild(  hull  ); 

boc^“>addChild (  silver  ); 
body->addChild(  thrusterFV  ); 
body->addChild (  thrusterAV  ); 
body->addChild(  thrusterFH  ); 
bo(d^->addChild(  thrusterAH  ); 

body->addChild(  cardcageleft  ); 
boc^->addChild  (  cardcageright  ); 
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///////////////////////////////////////////////////////////////////////////// 
//  construct  nosecone  using  a  NURBS  surface 

Static  float  pts  [25]  [3]  = 

{ 


{ 

- 

SEAM, 

LEFT, 

TOP 

}, 

{ 

- 

SEAM, 

LEFT_HALF 

,  TOP 

}, 

{ 

- 

SEAM, 

CENTER, 

TOP 

}, 

{ 

- 

SEAM, 

RIGHTHALF 

,  TOP 

}, 

{ 

- 

SEAM, 

RIGHT, 

TOP 

}, 

{ 

- 

SEAM, 

LEFT, 

TOPHALF 

}, 

{ 

- 

SEAM  + 

DOMECONTROLPTHALF,  LEFT  HALF, 

TOPHALF 

}, 

{ 

- 

SEAM  + 

DOMECONTROLPTHALF,  CENTER, 

TOPHALF 

}, 

{ 

SEAM  + 

DOMECONTROLPTHALF,  RIGHTHALF, 

TOPHALF 

}, 

{ 

- 

SEAM, 

RIGHT, 

TOPHALF 

{ 

- 

SEAM, 

LEFT, 

CENTER 

}, 

{ 

- 

SEAM  + 

DOMECONTROLPTHALF,  LEFT_HALF, 

CENTER 

}, 

{ 

- 

SEAM  + 

DOMECONTROLPT ,  CENTER , 

CENTER 

}, 

{ 

- 

SEAM  + 

DOMECONTROLPTHALF,  RIGHTHALF, 

CENTER 

}, 

{ 

- 

SEAM, 

RIGHT, 

CENTER 

{ 

- 

SEAM, 

LEFT, 

BOTTOMHALF 

}, 

{ 

- 

SEAM  + 

DOMECONTROLPTHALF,  LEFT  HALF, 

BOTTOMHALF 

}, 

{ 

- 

SEAM  + 

DOMECONTROLPTHALF,  CENTER, 

BOTTOMHALF 

}, 

{ 

- 

SEAM  + 

DOMECONTROLPTHALF,  RIGHTHALF, 

BOTTOMHALF 

}, 

{ 

SEAM, 

RIGHT, 

BOTTOMHALF 

{ 

SEAM, 

LEFT, 

BOTTOM 

}, 

{ 

SEAM, 

LEFT_HALF, 

BOTTOM 

{ 

- 

SEAM, 

CENTER, 

BOTTOM 

}, 

{ 

SEAM, 

RIGHTHALF, 

BOTTOM 

}, 

{ 

SEAM, 

RIGHT, 

BOTTOM 

static  float  knots  [10]  =  . 

{  0,  0,  0,  0.  0,  1,  1,  1,  1,  1  }; 

SoComplexity  *noseconeComplexity  =  new  SoComplexity; 
noseconeComplexity->valuG  =  0..  7; 

SoCoordinateS  *controlPts  =  new  SoCoordinateS; 
controlPts~>point.setValues  (  0,  25,  pts  ); 

SoNurbsSurface  *sonardoine  =  new  SoNurbsSurface; 
sonardome->nuinUControlPoints  .setValue  (5); 
sonardome- >nuinVControl Points  .  setValue  {  5  )  ; 
sonardomG~>uKnotVector.setValues  {  0,  10,  knots  ); 
sonardoine‘>vKnotVector.setValues  {  0,  10,  knots  )  ; 

SoSeparator  *nosesGction  =  new  SoSeparator; 
nosesection->addChild(  npsblue  ); 
nosesection->addChild(  noseconeComplexity  ) ; 
nosesection->addChild(  controlPts  ) ; 
nosesection“>addChild(  sonardome  ); 

If  Initialize  the  Cone  for  Representing  the  STIOOO  Beam 
coneSonarSTlOOO  =  new  SoCone;  If  global  for  callbacks 
if  (AUV_ST1000_range  >0.0) 

{ 

coneSonarST1000->height  =  f abs (AUy_ST1000_range) ; 

coneSonarST1000->bottomRadius  =  f abs {AUV_ST1000_range) /60 . 0 ;  //  1  degree 
sonar_cone_colorl000->  ambientColor. setValue  (  1.0,  0.0,  0.15  ); 
sonar_cone_colorl000->  diffuseColor. setValue  (  1.0,  0.0,  0.15  )i 
sonar_cone_colorl000->specularColor. setValue  (  1.0,  0.0,  0.15  ); 
ST1000Complexity->value. setValue  (  STlOOOComplexi ty_on  ); 
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} 

else  if  {AUV_ST1000_range  <  0,0) 

{ 

coneSonarST1000->height  =  0.001; 

coneSonarST1000“>bottomRadius  =  0.0001; 
xfConeSonarST1000-'>translation.  setValue 
sonar_cone_,colorl000->  ambientColor.  setValue 
sonar_cone_colorl000-'>  diffuseColor. setValue 
sonar_cone_colorl000->specularColor. setValue 


0.0, 

0.0, 

0.0)  ; 

0.0, 

0.0, 

0.0  ); 

0.7, 

0.7, 

0.0  ); 

0.7, 

0.7, 

0.0  ); 

} 


STl  00 OCompl exi ty- >value .  setValue  (  ST1000Coniplexity_of f 


else  //  zero  range,  draw  tiny  initialization  no-detection  cone 
{ 


coneSonarST1000“>height  =  0.001; 

coneSonarST1000->bottoniRadius  =  0.0001; 

sonar_cone_colorl000->  ainbientCol or .setValue  {  0.0,  0.0,  0.0  ); 
sonar_cone_colorl000->  diffuseColor. -setValue  (  0.7,  0.7,  0.0  ); 
sonar_cone_colorl000->specularColor. setValue  (  0.7,  0.7,  0.0  ); 
STl OOOCompl exi ty->value . setValue  (  STIO 0 OCompl exi ty_off  ); 

} 

coneSonarST1000->parts  =  SoCone :: SIDES; 


SoSeparator  *  sepSonarlOOO  =  new  SoSeparator; 

//  drawn  from  center 
if  (AUV_ST1000_range  >0.0) 

xfConeSonarST1000“>translat ion. setValue (  0.0, 

-  (AUV_ST1000_range/  2.0), 

0.0); 

else  xfConeSonafST1000->translation. setValue  (  0.0,  0.0,  0.0  ); 
if  (TRACE)  { 

cout  «  "xfConeSonarST1000->translation.x  =  "  «  xfConeSonarSTlOOO- 
>translation.getValue 0  [0]  «  endl; 

cout  «  "xfConeSonarST1000->translation.y  =  "  «  xfConeSonarSTlOOO- 
>translation.getValue 0  [1]  «  endl; 

cout  «  "xfConeSonarST1000->translation. z  =  "  «  xfConeSonarSTlOOO- 
>translation.getValue 0  [2]  «  endl; 

} 

of fsetST1000->translation. setValue (  AUV_ST1000_x_of fset  /  12.0, 

-AUV_ST1000_y_offset  /  12.0, 
-AUV_ST1000_z_offS€t  /  12.0); 


if  (TRACE)  { 

cout  «  "of fsetST1000->translation.x  =  " 

«  offsetST1000->translation.getValue() [0]  «  endl; 
cout  «  "of fsetST1000->translation.y  =  " 

«  of fsetST1000->translation. getValue ( ) [1 ]  «  endl; 
cout  «  "offsetST1000->translation. z  =  " 

«  offsetST1000->translation.getValue() [2]  «  endl; 

} 

sepSonarl000->addChild (  offsetSTlOOO  ); 
sepSonarl000->addChild(  ro90z  ); 
rotConeSonarST1000->axis  =  SoRotationXYZ : : Z; 
sepSonarl000->addChild{  rotConeSonarSTlOOO  ); 
sepSonarl000->addChild (  wires  ); 
sepSonarl000->addChild(  STlOOOComplexity  ); 
sepSonarl000->addChild{  sonar_cone_colorl000  ) ; 
sepSonarl000->addC]iild{  xfConeSonarSTlOOO  ); 
sepSonarl000->addChild{  coneSonarSTlOOO  ); 

/*  Initialize  the  Cone  for  Representing  the  ST725  Beam  */ 
coneSonarST725  =  new  SoCone;  //  global  for  callbacks 
if  (AUV_ST7 2 Strange  >0.0) 

{ 

coneSonarST725“>height  =  fabs  {AUV_ST725_range) ; 

coneSonarST725“>bottomRadius  =  fabs  (AUV_ST725_range)  /  60.0;  //  1  degree 
sonar_cone_color725->  ambientColor. setValue  (  0.8,  0.0,  0.8  ); 
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sonar__conG_color725->  diffuseColor.setValue  (  0.8,  0.0,  0.8  ); 
sonar_cone__color725”>specularColor.setValue  (  0.8,  0.0,  0.8  }; 
ST725Complexity->value.setValue  (  ST725Coinplexity_on  )  ; 

) 

else  if  {AUV_ST725_range  <  0.0) 

{ 

coneSonarST725->height  =  0.001; 

coneSonarST725->bottomRadius  =  0.0001; 
xfConeSonarST725->translation.setValue  (  0.0,  0.0,  0.0); 
sonar_cone_color725->  ambientColor . setValue  (  0.0,  0.0,  0.0  ); 

sonar_cone__color725“>  diffuseColor.setValue  (  0.0,  0.0,  0.0  ); 

sonar__cone_color725->specularColor.setValue  {  0.7,  0.7,  0.7  ); 
ST725Coinplexity->value.setValue  {  ST725Complexity_of f  ); 

} 

else  //  zero  range,  draw  tiny  initialization  no-detection  cone 
{ 

coneSonarST725->height  =  0.001; 

coneSonarST725->bottoinRadius  =  0.0001; 

sonar_cone_color725->  ainbientColor.setValue  (  0.0,  0.0,  0.0  ); 

sonar_conG_color725->  dif fuseColor.setValue  (  0.0,  0.0,  0.0  ); 

sonar_cone_color725->specularColor.detValue  (  0.7,  0.7,  0.7  ); 
ST725Coinplexity->value.  setValue  (  ST725Complexity_of f  ); 

coneSonarST725->parts  =  SoCone :: SIDES; 

//  drawn  from  center 
if  (AUV_ST725_range  >0.0) 

xfConeSonarST725->translation. setValue (  0.0, 

-  (AUV_ST725_range/  2.0), 

0.0); 

else  xfConeSonarST725->translation. setValue  (  0.0,  0.0,  0.0  ); 

SoSeparator  *  sepSonar725  =  new  SoSeparator; 

of fsetST725->translation. setValue (  AUV__ST725_x_of fset  /  12.0, 

-AUV_ST72  5^_offset  /  12.0, 
-AUV_ST725_z_offset  /  12.0); 
sepSonar725->addChild(  offsetST725  ) ; 
sepSonar725->addChild(  ro90z  ); 
rotConeSonarST725->axis  =  SoRotationXyZ::Z; 
sepSonar725->addChild(  rotConeSonarST725  ); 
seal eConeSonarST725->scaleFactor .setValue  (  1.0,  1.0,  12.0  ); 
sepSonar725->addChild(  scaleConeSonarST725  ); 
sepSonar725->addChild{  wires  ); 
sepSonar725->addChild(  ST7 2 5 Complexity  ) ; 
sepSonar725->addChild(  sonar_cone_color725  ); 
sepSonar725->addChild(  xfConeSonarST725  ); 
sepSonar725->addChild{  coneSonarST725  ); 

///////////////////////////////////////////////////////////////////////////// 
//  Define  tail  section 

//  ensure  polygons  are  defined  in  clockwise  fashion! 

//  Two  triangles  using  SoTriangleStripSet : 

static  int32_t  numbertrianglevertices  [2]  =  {3,  3}; 

//  static  long  numbertrianglevertices  [2]  =  {3,  3}; 

//  type  changed  from  long  to  int32_t  for  Inventor  2.1  upgrade 

static  float  af ttrianglevertices  [6] [3]  = 

{ 

{STERN,  LEFT,  0.0},  (SEAM,  LEFT,  TOP},  (SEAM,  LEFT,  BOTTOM), 

(STERN,  RIGHT,  0.0),  (SEAM,  RIGHT,  BOTTOM),  (SEAM,  RIGHT,  TOP), 


//  Define  coordinates  for  triangular  vertices  &  SoTriangleStripSet 
SoCoordinate3  *tailcoordl  =  new  SoCoordinate3; 
tailcoordl->point.setValues  (0,  6,  afttrianglevertices) ; 


SoTriangleStripSet  *tailtriangleset  =  new  SoTriangleStripSet ; 
tailtriangleset->niimVertices . setValues  (0,  2,  numbertrianglevertices) ; 


If  Two  rectangles  using  FaceSet; 

static  int32_t  numberquadvertices  [2]  =  {4,  4};  //  ref.  p. 
1 1  static  long  nmnberquadvertices  [2]  =  {4,  4);  //  ref.  p. 
//  type  changed  from  long  to  int32_t  for  Inventor  2.1  upgrade 


5-6 

5-6 


static  float  af tquadvertices  [8] [3]  = 

{ 

{STERN,  LEFT,  0.0},  (STERN,  RIGHT,  0.0),  (SEAM,  RIGHT,  TOP), 

(SEAM,  LEFT,  TOP), 

(STERN,  RIGHT,  0.0],  (STERN,  LEFT,  0.0},  (SEAM,  LEFT,  BOTTOM}, 

(SEAM,  RIGHT,  BOTTOM}, 

}; 


//  Define  coordinates  for  quad  vertices  &  SoFaceSet 
SoCoordinate3  *tailcoord2  =  new  SoCoordinate3; 
tailcoord2->point . setValues  (0,  8,  af tquadvertices ) ; 

SoFaceSet  *tailquadset  =  new  SoFaceSet; 


tailquadset->numVertices . setValues  (0,  2,  numberquadvertices) ; 

//  Two  cylinders  currently  represent  the  propellers  -  improve  this! 

//a  much  fancier  individual  prop  model  possible  here;  also  add  complexity 
SoCylinder  *prop  =  new  SoCylinder; 
prop->radius  =  2.00; 
prop->height  =  1.00; 


//  Two  cylinders  to  represent  the  shafts 
SoCylinder  *shaft  =  new  SoCylinder; 
shaf t->radius  =  0.50; 
shaf t->height  =  4.00; 

SoTransform  *xfl8  =  new  SoTransform;  //  shafts  relative  to  props 

//  note:  rotated  90z 

xfl8->translation.setValue{0.0,  -2.0,  0.0) ; 

SoTransform  *xfll  =  new  SoTransform;  //  left  prop 
xfll->translation. setValue (STERN  -  2.0,  SHAFTOFFSETLEFT,  0.0); 

//  compose  shaft  with  prop 
SoSeparator  *leftprop  =  new  SoSeparator; 
leftprop->addChild(  xfll  ); 
leftprop->addChild(  ro90z  ); 
leftprop->addChild{  prop  ); 
leftprop->addChild(  xfl8  ); 
leftprop->addChild(  shaft  ); 


SoTransform  *xf PropellerWakePort  =  new  SoTransform; 
xfPropellerWakePort->translation. setValue (  0.0,  15.0,  0.0  ); 

SoSeparator  *  separatorPropellerWakePort  =  new  SoSeparator; 
separatorPropellerWakePort->addChild(  xf PropellerWakePort  ); 
separatorPropellerWakePort->addChild{  rolSOx  ); 
separatorPropellerWakePort->addChild(  wires  ); 
separatorPropellerWakePort->addChild{  wake Complexity  ); 
separatorPropellerWakePort->addChild(  seagreen  ); 
conePropellerWakePort  =  new  SoCone;  //  global  for  callbacks 
conePropellerWakePort->height  =  AUV_port_rpm  /  100.0  *  24.0; 

conePropellerWakePort->bottoinRadius  =  fabs  (AUVjort_rpm)  /  100.0  *  6.0; 
conePropellerWakePort->parts  =  SoCone :: SIDES; 

separatorPropellerWakePort->addChild(  conePropellerWakePort  ); 
leftprop->addChild(  separatorPropellerWakePort  ); 

SoTransform  *xfl2  =  new  SoTransform;  //  right  props 
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xfl2->translation. setValue (STERN  -  2,0;  SHAFTOFFSETRIGHT,  0.0); 

SoSeparator  *righi:prop  =  new  SoSeparator; 
rightprop->addChild(  xfl2  ); 
rightprop->addChild(  ro90z  ); 
rightprop->addChild(  prop  ); 
rightprop->addChild{  xfl8  ) ; 
right:prop->addChild(  shaft  ); 

SoTransform  *xfPropellerWakeStbd  =  new  SoTransform; 
xfPropellerWakeStbd->translation.setValue(  0.0,  15,0,  0.0  ); 

SoSeparator  *  separatorPropellerWakeStbd  =  new  SoSeparator; 
separatorPropellerWakeStbd->addChild(  xf Propel lerWakeStbd  ); 
separatorPropellerWakeStbd“>addChild(  rolSOx  ); 
separatorPropellerWakeStbd“>addChild{  wires  ); 
separatorPropellerWakeStbd->addChild(  wakeComplexity  ); 
separatorPropellerWakeStbd~>addChild{  seagreen  ); 
conePropellerWakeStbd  =  new  SoCone;  //  global  for  callbacks 
conePropel lerWakeStbd- >height  =  AUV_port_rpm  /  100.0  *  24.0; 

conePropellerWakeStbd->bottomRadius  =  fabs  (AUV_port_rpm)  /  100.0  *  6.0; 
conePropel lerWakeStbd- >parts  =  SoCone :: SIDES; 

separatorPropellerWakeStbd->addChild{  conePropellerWakeStbd  ); 
rightprop->addChild(  separatorPropellerWakeStbd  ); 

//  construct  stern  box  support  beneath  aft  vertical  fins 

SoCube  *sternf insupportcube  =  new  SoCube; 

sternf insupportcube->width  =  FINLENGTH  +  1.5; 

sternf insupportcube->height  =5.0; 

sternf insupportcube->depth  =  HULLBODYHEIGHT  -  0.5; 

SoTransform  *xfl7  =  new  SoTransform; 
xf 17->translation . setValue (FINOFFSETAFT,  0.0,  0.0); 

SoSeparator  *sternf insupport  =  new  SoSeparator; 

sternf insupport->addChild(  xfl7  ); 

sternf insupport->addChild(  sternfinsupportcube  ); 

SoSeparator  *tailsection  =  new  SoSeparator; 
tailsection->addChild{  npsblue  ); 
tailsection->addChild(  tailcoordl  ); 
tailsection->addChild(  tailtriangleset  ) ; 
tailsection->addChild(  tailcoord2  ) ; 
tailsection->addChild(  tailquadset  ) ; 
tailsection->addChild(  sternf insupport  ); 
tailsection->addChild{  brass  ); 
tailsection->addChild(  leftprop  ); 
tailsection->addChild(  rightprop  ) ; 

///////////////////////////////////////////////////////////////////////////// 
//  robot  is  just  units  &  body  &  nosesection  &  tailsection  &  extra  stuff 

SoSeparator  *  robot  =  new  SoSeparator; 

//  iwiew/ivquicken  fails  on  SoUnits  nodes  so  SoScale  nodes  used  originally 
//  robot->addChild(  unitsfeet  ); 

//  robot->addChild(  MetersToFeet  ); 

//  robot  root  transform  for  overall  vehicle  orientation 
AUV_position_node  =  new  SoTransform; 

AUV_posi tion_node->translation . setValue (0.0,  0.0,  0.0); 
robot->addChild(  AUV_position_node  ); 

rotate_AUV_z  =  new  SoRotationXYZ; 
rotate_AUV_z->angle. setValue  (  -  AUVjsi); 
rotate_AUV_z->  axis . setValue  (SoRotationXYZ::Z); 
robot->addChild(  rotate_AUV_z  ); 

rotate__AUV_y  =  new  SoRotationXYZ; 
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rotate_AUV_y->angle.setValue  (  -  AUV_theta) ; 
rotate_AUV_y->  axis.setValue  (SoRotationXYZ: :Y) ; 
robot->addChild(  rotate_AUV_y  ) ; 

rotate_AUV_x  =  new  SoRotationXYZ; 
rotate_AUV_x->angle.setValue  (  AUV_phi); 
rotate_AUV_x->  axis.setValue  {SoRotationXYZ::X); 
robot->addChild(  rotate_AUV_x  ); 

robot->addChild(  sepSonar725  );  //  feet 

robot->addChild(  sepSonarlOOO  )  ; 

//  ivview/ivquicken  fails  on  SoUnits  nodes  so  SoScale  nodes  used  originally 
SoUnits  *unitsinches  =  new  SoUnits; 
unltsinches->units.setValue  (  SoUnits :: INCHES  ) ; 
robot->addchild(  unitsinches  ); 

//  robot ->addChi Id (  FeetToInches  ); 

SoPickStyle  *  unpickablestylenode; 

SoPickStyle  *  pickablestylenode; 
unpickablestylenode  =  new  SoPickStyle; 

pickablestylenode  =  new  SoPickStyle; 
unpickablestylenode->style.setValue  {  SoPickStyle: rUNPICKABLE  ); 
pickablestylenode->style.setValue  (  SoPickStyle: : SHAPE  ); 

//  Make  subsequent  nodes  unpickable  so  that  AUV  is  treated  as  a  whole 
robot->addChild(  unpickablestylenode  ); 

robot->addChild(  body  ); 
robot- >addChi Id (  nosesection  ); 
robot->addChild(  tailsection  ); 

/* 

SoTransforin  *xfl9  =  new  SoTransform; 

xf 19->translatlon. setValue {  0.0,  0.0,  -  2.0); 

robot->addChild(  xfl9  ); 

robot->addChild{  new  SoPointLight  ); 

cout  «  "new  point  light  added  under  robot"  «  endl; 


return  robot; 


///////////////////////////////////////////////////////////////////////////// 
double  sign  {double  x) 


{ 

if  (X  >  0.0)  return  1.0; 

elseif  (X  <  0,0)  return  ~1.0; 

else  return  1.0; 

} 

- - - 


double  degrees  (double  x)  //  radians  input 
{ 

return  x  *  180.0  /  PI; 


double  radians  (double  x)  //  degrees  input 
return  x  *  PI  /  180.0; 


// 


// - 

double  arcclainp  (double  x) 
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if  (X  >  1.0) 

{ 

X  =  1.0; 

cout  «  "viewer:  arcclamp  reduced  "  «  x  «  "  to  1.0"  «  endl; 

} 

else  if  (X  <  -I . 0) 

{ 

X  =  ”-1.0; 

cout  «  "viewer:  arcclamp  raised  "  «  x  «  "  to  -1.0"  «  endl; 
return  x; 

} 

// - // 

double  dnormalize  {double  angle__radians) 

{ 

double  new_angle  =  angle_radians; 

while  (new_angle  >  2*PI)  new_angle  -=  2*PI; 
while  (new__angle  <0.0  )  new_angle  +=  2*PI; 
return  new__angle; 

) 

// - 

-// 

void  swap_float  (float  a,  float  b) 

{ 

float  temp; 
temp  =  b; 
b  =  a; 
a  =  temp; 

} 

///////////////////////////////////////////////////////////////////////////// 

// - // 

static  int  DIS_net_open  ()  //  Ref;  macedonia  include  files 

{ 

//  Multicast  Defaults  from 

//  /n/elsie/work3/macedoni/net/mcast/network/utils/planes/planes.cc 

//  ttl  value  does  not  matter  since  this  viewing  program  only  reads  PDUs 
u_char  ttl  =1;  //  multicast  ttl=  1  stays  inside  LAN 

//  multicast  ttl=  15  stays  inside  NPS 
//  multicast  ttl=127  is  global 

int  exercise_id  =  -1; 

int  coordinate_system  =0;  //  0  =  flat  world,  1  =  round  world 

char  *  utm_file  =: 

int  result  =  net_op€n  (port,  group,  ttl, 

exercise_id,  coordinate_system,  utm_file) ; 

//  int  result  =  net_open  (port,  group,  ttl);  //  old  version 

if  (result  ==  FALSE) 

{ 

cout  «  "viewer:  DIS_net_open  ()  failed"  «  endl; 

DIS_port_open  =  FALSE; 

} 

else 

{ 

DIS_port_open  =  TRUE; 

//  cout  «  "viewer:  port  =  "  «  port  «  ",  group  =  "  «  group 

//  «  "/  ttl  =  "  «  ttl  «  endl; 

//  cout  «  "  exercise_id  =  "  «  exercise_id 
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//  «  ",  coordinate_systein  =  "  «  coordinate^system 

//  «  ",  utm^file  =  «  endl; 

} 

return  result; 

} 

// - // 

static  void  DIS_Redraw_Callback  (  void  *  unused__data, 

SoSensor  *  unused_calling_sensor  ) 

{ 

unused_data  =  unused_data; 

unused__calling_sensor  =  unused_calling_s€nsor;  //  eliminate  warnings 

double  delta_x  =  0.0; 

double  delta_y  =  0.0; 

double  delta_2  =  0.0; 

double  delta_phi  =  0.0; 

double  delta_theta  =  0.0; 

double  delta_psi  =  0.0; 

int 

//  EntitylD 
//  EntityType 
EntityStatePDU 
char 
PDUType 

//  static  int  rcvd  =  0; 

char  NPSAUV_Marking  [11]; 

bzero  (NPSAUV_Marking,  11); 

strncpy  (NPSAUV_Marking,  "Phoenix  AUV",  11);  //  11  chars  max 

if  ((delta_time  <=  5.5)  &&  (delta_time  >=  0.0)  &&  (DEADRECKON  ==  TRUE)) 

{ 

//  cout  «  "viewer;  DIS__Redraw__Callback :  PDU  loop  delta__time  =  " 

//  «  delta_time  «  endl; 

} 

while  (TRUE)  //  until  break 
{. 

number_of_PDUs  =  net_read  {&  local_PDU,  &  local_PDU_type) ;  //  old  version 

//  number_of_PDUs  =  net_read  (Sc  local_PDU,  &  local_PDU_type,  &  inaddr)  ; 

//  cout  «  "viewer:  net_read  complete,  number_of_PDUs  available  =  " 

//  «  nuinber__of_PDUs  «  endl; 

if  (number_of_PDUs  <=  -1) 

cout  «  "viewer:  Error  on  net_read,  number_of_PDUs  =  " 

«  number_of__PDUs  «  endl; 

else  if  (nuinber_of_PDUs  <=  0) 

{ 

break;  //no  more  PDUs,  done  for  now 

} 

//  rcvd  ++; 

//  cout  «  "PDU  received,  rcvd  =  "  «  rcvd  «  endl; 

//  printPDU  (local_PDU); 

UUV_DIS_j)du  =  (EntityStatePDU  *)  local_PDU; 

//  ensure  the  PDU  values  are  the  right  types 


number_of_PDUs ; 

UUV_DIS_id; 

UUV_DIS_type; 

*  UUV_DISjidu  =  NULL; 

*  local_PDU  =  NULL; 
local_PDU_type ; 
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if  {local__PDU_type  !=  EntityStatePDU^TVpe) 

{ 

cout  «  "viewer:  local_PDU_type  i=  EntityStatePDU^'IVpe,  ignored..." 
«  endl ; 

printPDU  (local_PDU) ; 

//  don't  forget  freePDU  or  get  a  memory  leak! 

//  articulated  parameters  are  also  freed 
freePDU  ((char  *)  UUV_DIS_pdu) ; 

cout  «  "viewer:  freePDU  ((char  *)  UUV_DIS_pdu)  called  for  this  PDU" 

«  endl; 

continue;  //  not  a  break  since  other  PDUs  may  be  waiting 

else  if  (strncmp  ((char  *)  UUV_DI S__pdu->entity_marking. markings , 

NPSAUV_Marking,  11)  !=  0) 

{ 

cout  «  "viewer:  non-NPS  AUV  Entity  State  PDU  encountered,  ignored" 
«  endl; 

printPDU  (local_PDU) ; 
freePDU  ((char  *)  UUV_DIS_^du) ; 

//  don't  forget  freePDU  or  get  a  memory  leak! 

//  articulated  parameters  are  also  freed 
cout  «  "viewer:  freePDU  ((char  *)  UUV_DIS_pdu)  called  for  this  PDU" 

«  endl; 

continue;  //  not  a  break  since  other  PDUs  may  be  waiting 
If  cout  «  "PDU  OK"  «  endl; 

//  extract  parameters  of  an  entity  state  PDU  (most  are  listed  in  pdu.h) 
//  this  assumes  there  are  no  articulated  parameters  (add  later)  «< 

//  DIS  ID  and  Type 

//  UUV_DIS_id  =  UUV_DISjdu->entity_id; 

//  UUV_DIS_type  =  UUV_DIS_pdu->entity_type; 

time_stamp_of__current_PDU  =  UUV_DIS_pdu->entity__state_header.time_stamp; 
time_of_PDU_receipt  =  clock  ( ) ; 

PDU_overdue  =  FALSE; 

//  Posture 
AUV_x 
AUV_y 
AUV_z 
AUV_phi 
AUV_theta 
AUV_psi 


// 

cout 

« 

endl  ; 

// 

cout 

« 

"viewer:  DIS  net  read  posture  trace:"  « 

endl  ; 

cout 

« 

"  ["; 

cout 

« 

UUV_D  I  S_pdu  “  >en  t  i  ty_l  oca  t  i  on .  X 

«  ", 

it  • 
/ 

cout 

« 

UUV_DIS_jpdu->entity_location  .y 

«  "  / 

//  . 
/ 

cout 

« 

UUV_DIS_pdu->entity_location .  z 

«  ", 

cout 

« 

UUV_DI  S_pdu  *- >en  t  i  ty_o  r  i  en  t  a  t  i  on .  ph  i 

«  " , 

cout 

« 

UUV_DIS_j)du->entity_orient at ion. theta 

«  " , 

cout 

« 

UUV_DIS_pdu->entity__orientation.psi 

«  "] 

" ; 

//  cout  «  endl; 


//  Linear  &  angular  velocities  in  body  coordinates/meters  by  DIS  standard 
AUV_x_dot  =  UUV_DISjidu->entity_velocity .X  *  FT_PER__METERS ; 

AUV__y_dot  =  UUV__DIS_pdu->entity_velocity .y  *  FT_PER_METERS ; 

AUV_z_dot  =  UUV_DIS_pdu->entity_velocity.  z  *  FT_PERJyIETERS; 

AUV__phi_dot  = 

radians  (UUV_DIS_pdU“>dead_reckon_params.angular_velocity  [0] )  ; 


=  UUV_DISjdu->entity_location.x  *  FT_PER_METERS ; 

=  UUV_DIS_pdu->entity_location.y  *  FT_PERjyiETERS ; 

=  UUV_DIS_pdu->entity_location.z  *  FT_PER_METERS; 

=  radians  (UUV_DIS_pdu->entity_orientation.phi) ; 

=  radians  (UUV_DIS_pdu->entity_orientation. theta) ; 

=  radians  (UUV_DIS_pdU”>entity_orientation .psi ) ; 
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AUV__t  he  t  a_do  t = 

radians  (UUV_DIS_pdu->dead_reckon_params .  angular_velocity  [1  ] )  ; 
AUV__psi__dot  = 

radians {UUV_DIS_pdu->dead_reckon_parains.angular_velocity [2] ) ; 

//  cout  «  "viewer:  World  coordinate  velocities: 

//  cout  «  " ["; 

//  cout  «  UUV_DIS_pdU“>entity_veloci ty  ,x  «  ", 

//  cout  «  UUy_DIS_pdU“>entity_velocity .y  «  ", 

//  cout  «  UUV_DIS_jpdU“>entity_velocity .  z  «  ",  "; 

//  cout  «  UUV_DIS_iDdu->dead_reckon_paraiTis , angular_velocity 

//  cout  «  UUV_DIS_pdu->dead_reckon_parains . angular_velocity 

//  cout  «  UUV__DIS_pdu->dead_reckon_params . angular_veloci ty 

/  /  «  endl  ; 

//  cout  «  endl; 

//  Note  even  though  the  accelerations  are  calculated  in  the  superclass 
//  UUVBody,  use  of  global  state  vector  lets  us  construct  class  hierarchy 
//  based  on  problem  structure  instead  of  the  communications  dependencies. 
//  This  is  proposed  as  a  general  DIS-compatible  vehicle  object  hierarchy. 

//  Accelerations  not  produced  in  world  coordinates,  thus  zeroes  expected 
AUV_u_dot  = 

UUV_DIS_pdu->dead_reckon_jparams .  linear_accel  [ 0 ]  *  FT_PER_METERS ; 
AUV_v_dot  = 

UUV_DIS_pdu->dead_reckon_params . linear_accel  [ 1 }  *  FT_PER_METERS ; 
AUV_w_dot  = 

ljav_DIS_pdu->dead_reckon_params. linear_accel  [2]  *  FT_PER_METERS ; 


// 

cout  « 

"viewer:  World  coordinate  accelerations:  "; 

// 

cout  « 

// 

cout  « 

UUV_DIS_pdu->dead_reckon_params . linear__accel 

[0] 

«  ",  "; 

// 

cout  « 

UUV__DIS_pdu->dead__reckon_params .  linear_accel 

[1] 

«  ",  "; 

// 

endl ; 

cout  « 

UUV_DIS_pdu->dead_reckon_params .  linear_accel 

[2] 

«  "  ]  "  « 

[0]  «  ", 
[IJ  «  ^ 
[2]  «  "]" 


//  what  we  look  like 
//  UUV_DIS__pdu->entity_appearance; 

/ /  UUV_DIS_jjdu->ent ity_marking . character_set ; 

/  /  UUV__D  I  S_pdu  -  >ent  i  ty_ma  r k  i  ng .  ma  rk  i  ngs  ; 

//  project  our  movement 

UUV_DIS_pdu->dead_reckonjarams.  algorithm  =  DRAlgo_DRM_FPW; 

//  cout  «  "viewer:  DIS_net_read  {)  successful"  «  endl; 

//  cout  «  flush; 


//  update  overall  AUV  posture  (both  position  &  orientation) 

//  account  for  shift  to  SGI/OpenInventor  coordinate  system 
AUV_position_node“>translation.setValue  (  AUV_x,  -  AUV_y,  ~  AUV_z); 
rotate_AUV_z->  angle. setValue  (  ~  AUV_psi); 

rotate_AUV_:y->  angle .  setValue  (  ->  AUV_theta) ; 

rotate_AUV_x->  angle .  setValue  (  AUV_phi ) ; 


ArticulatParamsNode  *  APNptr  =  UUV_DIS_pdu^>articulat_params_head; 


AUV_time  = 

AUV_time  += 

AUV__delta_rudder  = 
AUV__delta_planes  = 


APNptr->articulat_params.parameter_value  [0] ; 
APNptr- >articulat_params.parameter_value  [1]  /  10.0; 
APNptr->articulat_params.parameter_value  [2] ; 
APNptr->articulat_params.parameter_value  [3] ; 


//  denormal ize  former  shorts  to  be  negative  if  necessary 
if  (AUV_delta_rudder  >=  128.0)  AUV_delta_rudder  -=  256.0; 
if  (AUV_delta_p lanes  >=  128.0)  AUV_delta_p lanes  256.0; 


AUV_port_rpm  =  APNptr->articulat_params . parameter_value  [4]; 

if  (AUV_jDort_rpm  >=  128.0)  AUV_port_rpm  =  AUV_port_rpm  -  256.0; 
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AUV_port_rpm  *=10.0; 

i  f  { AUV _jpo  r  t_rpin  >=  0.0) 

AUV_port_rpm  +=  APNptr“>articulat_paraing.paraineter_value  [5]; 
else  AXJV_port_rpm  -=  APNptr->articulat_i)arams .pararaeter_value  [5]; 

AUV_stbd_,rpin  =  APNptr->articulat_parains .paraineter_value  [6]; 

if  (AUV_stbd_rpm  >=  128.0)  AUV__stbd_rpin  =  AUV_stbd_rpm  “  256.0; 

AUV_s  tbd^rpm  *  =  10.0; 

if  {AUV_stbd_rpm  >=0.0) 

AUV__stbd_rpin  +=  APNptr->articulat_params .parameter_value  [7]; 
else  AUv_stbd_rpm  ~=  APNptr“>articulat_params  .parameter^value  [7]; 

cout  «  «  AUV^time  «  ")"  «  endl; 

//  cout  «  "viewer:  Articulation  parameter  0:"  «  endl; 

//  cout  «  "AUV__time  =  "  «  AUV_time  «  endl; 

//  cout  «  "AUV_delta_rudder  =  "  «  AUV__delta_r udder  «  endl; 

//  cout  «  "AUV_delta_p lanes  =  "  «  AUV_delta_p lanes  «  endl; 

//  cout  «  "AUV_iDort_rpm  =  "  «  AUV__port_rpm  «  endl; 

//  cout  «  "AUV_stbd_rpm  =  "  «  AUV_stbd_rpm  «  endl; 

APNptr  =  APNptr->next_articulat_params;  //  next  articulated  parameter 

//  thrusters 

AUV__bow_vertical  =  APNptr->articulat_params .parameter^value  [Oj; 
AUV_stern__vertical  =  APNptr->articulat_params .parameter_value  [1]; 
AUV_bow_lateral  =  APNptr“>articulat_params  .parameter_value  121; 
AUV_stern_lateral  =  APNptr->articulat__params  .paraineter_value  [3]  ; 

//  denormaliz6  former  shorts  to  be  negative  if  necessary 
if  {AUV_bow_vertical  >=  128.0)  AUV_bow_vertical  -=  256.0; 

if  (AUV_stern_vertical  >=  128.0)  AUV_stern_vertical  -=  256.0; 

if  (AUV_bow_lateral  >=  128.0)  AUV_bow__lateral  -=  256.0; 

if  (AUV_stern_lateral  >=  128.0)  AUV_stern_lateral  -=  256.0; 

//  convert  thruster  volts  to  force  by  signed  squares  &  normalize  adjust 
AUV_bow_vertical  =  AUV_bow_vertical  *  fabs  (AUV_bow_vertical  )  /24.0; 

AUV_stern_vertical  =  AUV_stern_vertical*  fabs (AUV_stern_vertical )  /24.0; 

AUV_bow_lateral  =  AUV_bow_lateral  *  fabs  (AUV_bow_lateral  )  /24.0; 

AUV_stern_lateral  =  AUV_stern_lateral  *  fabs (AUV_stern_lateral  )  /24.0; 

//  slots  4.. 7  as  yet  unused 

//  cout  <<  "viewer:  Articulation  parameter  1:  thrusters" 

/  /  «  endl  ; 

//  cout  «  "AUV_bow_vertical  =  "  «  AUV_bow_vertical  «  endl; 

//  cout  «  "AUV_stern_vertical  =  "  «  AUV_stern_vertical  «  endl; 

//  cout  «  "AUV_bow_lateral  =  "  «  AUV_bow_lateral  «  endl; 

//  cout  «  "AUV_stern_lateral  =  "  «  AUV_stern_la.teral  «  endl; 

APNptr  =  APNptr->next_articulat_params;  //  next  articulated  parameter 

AUV_ST1000_bearing  =  APNptr“>articulat_params  .parameter_value  [O]  *  10  + 

APNptr->articulat_parains .parameter_valuG  [1]; 
AUV_ST1000_range  =  APNptr  “>articulat_params  .parameter_value  [2]  /  4.0; 
AUV_ST1000_strength  =  APNptr“>articulat_params .parameter_value  [31; 
AUV_ST725_bearing  =  APNptr->articulat_params .parameter_value  [41  *  10  + 

APNptr->articulat_parains .parameter__value  [51  ; 
AUV_ST7 2 5_range  =  APNptr->articulat_params .parameter_value  [61  /  4.0; 
AUV_ST725_strength  =  APNptr'->articulat_params .parameter_value  [7]; 

//  cout  «  "viewer:  Articulation  parameter  2:  sonar"  «  endl; 

//  cout  «  "AUV_ST1000_bearing  =  "  «  AUV_ST1000_bearing  «  endl; 

//  cout  «  "AUV_ST1000_range  =  "  «  AUV_ST1000_range  «  endl; 

//  cout  «  "AUV_ST1000_strength  =  "  «  AUV_ST1000_strength  «  endl; 

//  cout  «  "AUV_ST725_bearing  =  "  «  AUV__ST725_bearing  «  endl; 

//  cout  «  "AUV_ST725.range  =  "  «  AUV_ST7 2 Strange  «  endl; 

//  cout  «  "AUV_ST725_strength  =  "  «  AUV_ST725_strength  «  endl; 


//  initial  code  development  prior  to  porting  to  dynamics. C 


//  Print  hostname  of  PDU  (revision  in  network . round  version) 

//  hp  =  gethostbyaddr ( (char  *)  &inaddr,  sizeof (struct  in^addr) ,  AF_INET) 
//  cout  «  "viewer:  Host  name:  "  «  hp”>h_name  «  endl; 

//  don't  forget  freePDU  or  get  a  memory  leak! 

//  articulated  parameters  are  also  freed 

freePDU  (local_PDU) ; 

//  cout  «  "viewer:  freePDU  (local_PDU)  called  for  this  PDU"  «  endl; 

}  //  end  while  infinite  loop 

//  cout  «  "viewer:  DIS  net_read  portion  complete,  now  update  scene  graph." 
/  /  «  endl  ; 


current_clock 

delta_clock 

delta_time 


=  clock  0 ; 

=  current_clock  -  time_of_PDU_receipt; 

=  (double)  delta_clock  /  CLOCKS_PER_SEC; 


//  cout  «  "viewer:  current_clock .=  " 

//  «  ",  time_stamp_of_current_PDU  = 

//  «  "  /  time_of_PDU_receipt  =  " 

//  «  "/  PDU  delta  time  =  " 


«  current_clock 
«  time_stamp_of_current_PDU 
«  time_of_PDU_receipt 
«  delta__time  «  endl 


if 

tures 

{ 


((delta_time  >=  0,0)  &&  (delta_time  <=  5.0))  //  update  positions,  pos~ 


if 

{ 


(DEADRECKON  ==  TRUE) 


} 

else 
{ 


delta_x 

delta_y 

delta_2 

delta_phi 

delta_theta 

delta__psi 


delta_x 
delta_y 
delta_z 
delta_phi 
delta  theta 


=  AUV_x_dot 
=  AUV_jy_dot 
=  AUV_z_dot 
=  AUV_phi_dot 
=  AUV_theta_dot 
=  AUV_psi_dot 


0.0; 

0.0; 

0.0; 

0.0; 

0.0; 


*  delta_time; 

*  delta_time; 

*  delta_time; 

*  delta_time; 

*  delta_time; 

*  delta_time; 


} 

delta_psi  =  0.0; 

// 

cout 

« 

"viewer:  DeadReckon  Callback:  " 

// 

« 

"  PDU  delta  time 

if 

«  delta_time 

« 

endl  ; 

// 

cout 

« 

"  AUV^phi 

— .  if 

«  degrees 

(AUV  jphi ) 

// 

« 

",  AUV_phi_dot 

-w  if 

«  degrees 

{AUV_phi  dot) 

« 

endl  ; 

// 

cout 

« 

"  AUV  theta 

_  it 

«  degrees 

(AUV_theta) 

// 

« 

",  AUV_theta_dot 

—  tt 

«  degrees 

{AUV_theta_dot ) 

« 

endl  ; 

// 

cout 

« 

"  delta_x 

_  a 

«  delta_x 

« 

endl  ; 

// 

cout 

« 

"  delta_y 

=  " 

«  delta_y 

« 

endl  ; 

// 

cout 

« 

"  delta_2 

_  u 

<<  delta_z 

« 

endl ; 

// 

cout 

« 

endl ; 

//  save  current  AUV  position  for  next  time  the  camera  is  repositioned 
AUV_x_prior  =  AUV_x; 

AUVj/_prior  =  AUV_y; 

AUV_z_prior  =  AUV_z; 


//  graphics  rendering  problems:  psi,  rudders  are  opposite 
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//  update  overall  AUV  posture  {both  position  &  orientation) 
AUV_position_node“>translation,setValue  (  AUV^x  +  delta_x, 

-  (AUV_y  +  delta.jy')  , 

-  (AUV_z  +  delta^z)); 

rotate_AUV_z->angle.setValue  (  -  (AUV__psi  +  deltajsi)); 

rotatG_AUV_^->anglG.setValue  (  -  (AUV_theta  +  delta_theta)  )  ; 

rotate_AUV_x->angle.setValuG  (AUVjhi  +  deltajphi)  ; 

//  update  AUV  rudder  &  plane  orientations 

rotate_AUV_bow_rudders->angle .  setValue  (  radians  (AUV_del  ta_rudder )  )  ; 

rotate_AUV_stern_rudders->angle.setValue  {  -  radi¬ 
ans  (AUV_delta_r udder) ) ; 

rota te_AUV__bow_planes->angle. setValue  (  -  radians(AUV_delta_planes)); 
rotate_AUV_stern_planes->anglG . setValue  (  radi¬ 
ans  (AUV_delta_planes) ) ; 

//  cout  «  "AUV__delta_rudder  =  "  «  AUV_delta_ rudder 
//  «  ",  radians  (AUV_del ta_rudder)  =  " 

//  «  radians  (AUV_delta_ rudder) 

//  «  endl; 

//  cout  «  "AUV_dGlta_planes=  "  «  AUV__delta_p lanes 
//  «  ",  radians  {AUV_delta_planes)  =  " 

//  «  radians  (AUV_delta_p lanes) 

//  «  endl; 

if  (fabs  (AUV__stbd_rpin  - 

(conePropellerWakeStbd->hGight.getValue{)  *  100.0/24,0))  >  10.0) 
//  ensure  needed 
{ 

conGPropellGrWakeStbd->hGight  =  AUV_stbd_rpin  7100.0*24.0; 

^  conePropellerWakeStbd->bottoinRadius  =  fabs  (AUV_stbd_rpni)  /lOO.O*  6.0; 

if  (fabs  (AUV_port_rpin  - 

{conePropellerWakePort->height.getValue()  *  100.0/24.0))  >10.0) 

//  ensure  needed 
{ 

conePropellerWakePort->height  =  AUV_j)ort_rpm  7100.0*24.0; 

conePropellerWakePort->bottomRadius  =  fabs  (AUV_port_rpin) /lOO . 0*  6.0; 

//  always  recalculate  thrusters 
{ 

thrusterWakGFV->height  =  -  AUy_bow__vGrtical  *  2.0; 

thrusterWakeFV->bottomRadius  =  AUV_bow_vertical  /  4.0; 

if  (AUV_bow__vertical  <  0.0) 

//  bottomsideBow,  negative  volts  push  up  (negative  direction) 
whichWakeFV->whichChild  =  1; 

//  topsideBow,  positive  volts  push  down  (positive  direction) 
else  whichWakeFV->whichChild  =  0; 

topsideBow->  translation . setValue (0 , TOPHALF+AUV_bow_vertical , 0 ) ; 

^  bot tomsidGBow->translat ion. setValue (0,BOTTOMHALF+AUV_bow_vertical , 0) ; 

//  always  recalculate  thrusters 
{ 

thrusterWakeAV->height  =  -  AUV_stern_vertical  *  2.0; 

thrusterWakeAV->bottomRadius  =  AUV_stern_vertical  /  4.0; 

if  (AUV_stern_vertical  <  0.0) 

//  bottomsideStern,  negative  volts  push  up  (negative  direction) 
whichWakeAV->whichChild  =  1; 

//  topsideStern,  positive  volts  push  down{positive  direction) 
else  whichWakeAV->whichChild  =0; 

topsideStGrn->translation.setValue(0,  TOPHALF+AUV_stern_yertical, 0) ; 
bottoinsideStern->translation. setValue (0,BOTTOPHALF+AUV_stern_vertical,0) ; 

//  always  recalculate  thrusters 
{ 
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thrusterWakeFH->height  =  -  AUV_bow_lateral  *2.0; 

thrusterWakeFH-'>bottoinRadius  =  AUV_bow_lateral  /  4.0; 
if  (AUV_bow_lateral  <  0.0) 

//  rightsideBow,  negative  volts  push  left  {negative  direction) 
whichWakeFH->whichChild  =  1; 

//  leftsideBow,  positive  volts  push  right  (positive  direction) 
else  whichWakeFH“>whichChild  =  0; 

leftsideBow->  translation. setValue  (0,  LEFT_HALF+AUV_bow_lateral ,  0) ; 
rightsideBow->translation.setValue  (0,  RIGHTHALF+AUV_bow__lateral ,  0); 

//  always  recalculate  thrusters 
{ 

thrusterWakeAH”>height  =  -  AUV_stern_lateral  *2.0; 

thrusterWakeAH->bottomRadius  =  AUy_stern_lateral  /  4.0; 
if  (AUV_stern_lateral  <  0.0) 

//  rightsideStern,  negative  volts  push  left  (negative  direction) 
whichWakeAH->whichChild  =1; 

//  leftsideStern,  positive  volts  push  right (positive  direction) 
else  whichWakeAH->whichChild  =  0; 

leftsideStern'->translation.setValue(0,LEFT__HALF+AUV__stern_lateral,  0)  ; 
rightsideStern>>translation .  setValue  { 0 ,  RIGHTHALF+AUV_stern_lateral ,  0 )  ; 

//  Process  ST725  Data  for  Cone  Representation 

if  (  (fabs  (AW_ST725_range  -  coneSonarST725->height  .getValue  ( )  )  >  0.0)  II 
{AUV_ST7  2  Strange  <  0.0)) 

//  ensure  needed 
{ 

if  (AUV_ST7 2 Strange  >0.0) 

{ 

coneSonarST725->height  =  fabs  (AUV_ST725_range) ; 

coneSonarST725->bottomRadius  =  fabs  (AUV_ST725_range)  /  60.0; 

//  1  degree 

xfConeSonarST725->translation. setValue  (  0.0, 

-  {AUV_ST7 2 Strange  /  2,0), 
0.0)  ; 

sonar_cone_color725->  ambientColor .setValue  (  0.8,  0.0,  0.8  ); 
sonar_cone_color725~>  diffuseColor. setValue  (  0.8,  0.0,  0.8  ); 
sonar_cone_color725->specularColor .setValue  {  0.8,  0.0,  0.8  ); 
ST72  5Coinplexity“>value. setValue  (  ST725Coinplexity_on  ); 

else  if  {AUV_ST725_range  <  0.0) 

{ 

coneSonarST725->height  =  0.001; 

coneSonarST725->bottomRadius  =  0.0001; 
xfConeSonarST725“>translation. setValue  (  0.0,  0.0,  0.0); 
sonar_cone_color725“>  ambientColor .setValue  (  0.0,  0.0,  0.0  ); 

sonar_cone_color725~>  diffuseColor. setValue  (  0.0,  0.0,  0.0  ); 

sonar_cone_color725->specularColor. setValue  (  0.7,  0.7,  0.7  ); 

^  ST725Complexity->value. setValue  (  ST725Complexity_off  ); 

else  //  zero  range  returned,  draw  full-size  no-detection  cone 
{ 

coneSonarST725->height  =  32.808; 

coneSonarST725->bottomRadius  =  0.5468;  //  bottom  of  cone  max  range 
xfConeSonarST725->translation. setValue  {  0.0,  -16.404,  0.0); 
sonar_cone_color725->  ambientColor .setValue  (  0.0,  0.0,  0.0  ); 

sonar_cone_color725->  diffuseColor. setValue  (  0.0,  0.0,  0.0  ); 

sonar_cone_color725->specularColor .setValue  {  0.7,  0.7,  0.7  ); 
ST725Complexity->value. setValue  (  ST725Complexity_of f  ); 


rotConeSonarST725->angle  =  radians  (  -AUV_ST725_bearing  ); 


//  Process  STIOOO  Data  for  Cone  Representation 
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if  { {fabs{AUV_ST1000_range  -  coneSonarST1000->height .getValue ( ) ) >0 . 0) M 
(AUV_ST1000_range  <  0.0)) 

//  ensure  needed 
{ 

if  (AUV_ST1000_range  >0.0) 

{ 

coneSonarST1000->height  =  fabs  (AUV_ST1000_range) ; 

coneSonarST1000->bottoinRadius  =  fabs  {AUV_ST1 00 Derange)  /  60.0; 

//I  degree 

xfConeSonarST1000-->translation . setValue  (  0.0, 

-  (AUV_ST1000_range  /  2.0), 
0.0); 

sonar_cone_colorl000->  ambientColor. setValue  (  1.0,  0.0,  0.15  ); 

sonar_cone_colorl000->  diffuseColor. setValue  (  1.0,  0.0,  0.15  ); 

sonar_cone_colorl000-->specularColor. setValue  {  1.0,  0,0,  0.15  ); 
ST1000Complexity->value. setValue  (  ST1000Complexity_on  ); 

} 

else  if  {AUV_ST1000__range  <  0.0) 

{ 

coneSonarST1000->height  =  0.001; 

coneSonarST1000->bottoinRadius  =  0.0001; 
xfConeSonarST1000->translation. setValue  (  0.0,  0.0,  0.0); 
sonar_cone_colorl000->  ambientColor. setValue  {  0,8,  0.8,  0.0  ); 

sonar_cone_colorl000->  diffuseColor. setValue  {  0.8,  0.8,  0.0  ); 

sonar_cone_colorl000->specularColor. setValue  (  0.8,  0.8,  0.0  ); 
ST1000Complexity->value.  setValue  (  STlOOOComplexity^of  f  ); 

else  //  zero  range  returned,  draw  full-size  no-detection  cone 
{ 

coneSonarST1000->height  =  32.808; 

coneSonarST1000->bottoinRadius  =  0.5468; 

xfConeSonarST1000->translation, setValue  (  0.0,  -16.404,  0.0); 
sonar_cone_colorl000->  ambientColor. setValue  (  0.8,  0.8,  0.0  ); 

sonar_cone_colorl000->  diffuseColor. setValue  (  0.8,  0.8,  0.0  ); 

sonar_cone_colorl000->specularColor. setValue  (  0.8,  0.8,  0.0  ); 
ST1000Complexity->value. setValue  (  ST1000Complexity_of f  ); 


if  (TRACE)  { 

cout  «  "xfConeSonarST1000->translation.x  =  " 

«  xfConeSonarST1000->translation.getValue{) [0]  «  endl; 
cout  «  "xfConeSonarST1000->translation.y  =  " 

«  xfConeSonarST1000->translation.getValue() [1]  «  endl ; 
cout  «  "xfConeSonarST1000->translation. z  =  " 

«  xfConeSonarST1000->translation.getValue() [2]  «  endl ; 

} 

rotConeSonarST1000->angle  =  radians  (  -AUV_ST1000_bearing  ) ; 


}  //  end  processing  current  AUV  PDU 

else  if  {PDU_overdue  ==  FALSE)  //  reset  vehicle  position,  update  scene  graph 

PDU_overdue  =  TRUE;  //  over  5  seconds  elapsed  since  last  PDU 

//  restore  latest  valid  AUV  posture  (both  position  &  orientation) 
AUV_position_node->translation. setValue  (  AUV_x, 

“  AUV_y, 

-  AUV_z ) ; 

rotate_AUV_z->angle. setValue  (  -  AUV_psi); 

rotate_AUV_y->angle.  setValue  (  -  AUV_theta);- 

rotate_AUV_x->angle. setValue  (  AUV_phi ) ; 

//  thrusters 

AUV_bow_vertical  =  0.0; 

AUV_stern_vertical  =  0.0; 

AUV__bow_lateral  =  0.0; 
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AUV_stern_lateral  =  0.0; 

0.0; 
0.0; 
0.0; 
0.0; 
0.0; 
0.0; 
0.0; 
0.0; 


AUV_ST1000_bearing  =  0; 

AUV_ST1000_range  =  -0.001; 

AUV_ST1000_strength  =  0; 

AUV_ST725_bearing  =  0; 

AUV_ST7 2  Strange  =  -0.001; 

AUV_ST725_strength  =  0; 


thrusterWakeFV->height  = 
thrusterWakeFV->bottoinRadius  = 
thrusterWakeAV->height  = 
t:hrusterWakeAV->bottomRadius  = 
thrusterWakeFH->height  = 
thrusterWak€FH->bottoinRadius  = 
thrusterWakeAH->height  = 
thrusterWakeAH->bottoiTiRadius  = 


coneSonarST725->height  =  fabs {AUV_ST7 2 Strange ) ; 

coneSonarST725->bottomRadius  =  fabs (AUV__ST7 2 Strange)  /60.0;  //I  degree 
coneSonarST1000->height  =  fabs (AUV_ST1000_range) ; 

coneSonarST1000->bottomRadius  =  f abs (AUV_ST1000__range) /60 . 0 ;  //I  degree 
xfConeSonarST725->translation.setValue  (  0.0,  0.0,  0.0); 
xfConeSonarST1000->translation,setValue  (  0.0,  0.0,  0.0); 


if  (TRACE)  { 

cout  «  "xfConeSonarST1000->t:ranslation,x  =  " 

«  xfConeSonarST1000->translacion.getValue{) [0]  «  endl ; 
cout  «  ".xfConeSonarST1000->translation,y  =  " 

«  xfConeSonarST1000->translation.getValue{) [1]  «  endl ; 
cout  «  "xfConeSonarST1000->translation. z  =  " 

«  xfConeSonarST1000->translation.getValue() [2]  «  endl ; 


AUV_port_rpm  =  0.0; 
AUV__stbd_rpin  =  0.0; 


conePropellerWakePort->heiglit  =  0.0; 
conePropellerWakePort->bottoinRadius  =  0.0; 
conePropellerWakeStbd->height  =  0.0; 
conePropellerWakeStbd->bottoitiRadius  =  0.0; 


if  (DEADRECKON  ==  TRUE) 

{ 

cout  «  "viewer:  DeadReckon_Callback :  " 

«  "PDU  delta_time  =  "  «  delta^time  « 

«  endl; 

cout  «  "viewer  position/posture  reset  to  last  received  PDU."  «  endl; 
cout  «  endl; 
cout  «  flush; 

} 

} 

if  (  (del ta__t ime  >=  0.0)  &&  (delta__tiine  <=  5.0))  //  force  camera  update 
//  camera  control  - 


priorAUVPosition  =  SbVecSf 
currentAUVPosition  =  SbVecSf 

aheadOfAUVPosition  =  SbVec3f 

behindAUVPosition  =  SbVec3f 


(  AUV_x_prior,  -AUV_z_prior,  AUV_y_prior  ) ; 
(  AUV_x  *  METERS_PER__FT, 


-AUV_Z  *  METERS_PER  FT, 
AUV_y  *  METERS_PER_FT  ) 

; 

5.0  *  cos 

(  AUV_psi 

), 

2.0  *  sin 

(  AUV_theta 

), 

5.0  *  sin 

(  AUV_psi 

)  + 

0.5); 

-5.0  *  cos 

{  AUV_psi 

) 

-2.0  *  sin 

(  AUy_theta 

), 

-3.0  *  sin 

(  AUV  theta 

), 

-5.0  *  sin 

(  AUV_psi 

)  + 

0.5)  ; 
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switch  (  whichCamera  )  //  reposition  appropriate  cainera  as  needed 
{ 

case  C^ERA_FREE:  //  FREE  means  no  modifications  to  camera  position 
priorCameraPosition  =  PerspectiveCameraFree->position. getValue  (); 
priorCameraOf fset  =  priorCameraPosition  -  priorAUVPosi tion; 

currentCameraPosition=  priorCameraPosition; 
PerspectiveCameraFree->orientation .getValue (orientationRotationAxis , 

orientationRotationAngle) ; 

break; 

case  CAMERA_TO_AUV :  //  retain  camera  pos'n  relative  to  new  AUV  position 
priorCameraPosition  =  PerspectiveCameraToAUV->posi tion. getValue  (); 
priorCameraOf fset  =  priorCameraPosition  -  priorAUVPosition; 

//  verify  here 

priorCameraPosition  =  PerspectiveCameraToAUV->posi tion. getValue  (); 

//  PerspectiveCameraToAUV“>position . setValue  (  currentAUVPosition 

//  +  standardCameraOf f set  ); 

PerspectiveCameraToAtJV->pointAt  (  currentAUVPosition  ); 

currentCameraPosition  =  PerspectiveCameraToAUV->posi tion. getValue  {); 
PerspectiveCameraToAUV->orientat ion. getValue  (orientationRotationAxis, 

orientationRotationAngle  ) ; 

break; 


case  CAMERA__FROM__AUV :  //  retain  camera  position  looking  from  AUV  pos. 
priorCameraPosition=PerspectiveCameraFromAUV->position. getValue  ( ) ; 
priorCameraOffset  =  priorCameraPosition  -  priorAUVPosition; 

//  verify  here 

currentCameraPosition  = 

PerspectiveCameraFromAUV->position . setValue 


PerspectiveCaineraFromAUV->pointAt 

PerspectiveCameraFromAUV->orientat ion. getValue 
break ; 


(  currentAUVPosition  ); 

(  currentAUVPosition 
+ AUVCame r aO f f se t 
+behindAUVPosition  ) ; 

(  currentAUVPosition 
+ AUVCame raO f f s e t 
+aheadOfAUVPosition  ) ; 
(orientationRotationAxis, 
orientationRotationAngle  ) 


}  //  end  switch  (  whichCamera  ) 


/* 


//  print  out  all  camera  parameters 


cout  « 

endl  ; 

cout  « 

„ 

AUV_position 

=  <" 

« 

(AUV  X) 

« 

u  ts 

, 

« 

(AUV_y) 

« 

-,/  ,/ 

, 

« 

(AUV  z) 

« 

endl ; 

cout  « 
« 
« 
« 
« 

cout  « 
« 
« 
« 
« 

cout  « 
« 
« 
« 

cout  « 
« 
« 
<< 
« 


"currentAUVPosition  =  <"  «  currentAUVPosition  [0] 

"  «  currentAUVPosition  [1] 

"/  "  «  currentAUVPosition  [2] 

">  meters,  shifted  to  light  coordinate  system" 
endl  ; 

"aheadOfAUVPosition  =  <"  «  aheadOfAUVPosition  [0] 

"/  "  «  aheadOfAUVPosition  [1] 

",  "  «  aheadOfAUVPosition  [2] 

">  meters"  //  shifted  to  light  coordinate  system 

endl  ; 

"delta_AUV_position  =  <"  «  (AUV_x  -  AUV_x_prior) 

"/  "  «  (AUV_:y  -  AUV_/_prior) 

"  «  (AUV_z  ~  AUV_z_prior)  « 

endl; 

"del ta_CameraPosi tion  =" 

"  <"  «  (currentCameraPosition  [0]  -  priorCameraPosition 

",  "  «  (currentCameraPosition  [1]  -  priorCameraPosition 

",  "  «  (currentCameraPosition  [2]  -  priorCameraPosition 

">  "  «  endl; 


">  " 


[0]  ) 
[1]) 
[2]  ) 
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cout  «  "priorCameraPosition  =" 

«  "  <"  «  priorCameraPosition  [0] 

«  "  «  priorCameraPosition  [1] 

«  ",  "  «  priorCameraPosition  [2]  «  «  endl; 

cout  «  "priorCameraOff set 

«  "  <"  «  priorCameraOffset  [0] 

«  ",  "  «  priorCameraOffset  [1] 

«  ",  "  «  priorCameraOffset  [2]  «  ">"  «  endl; 
cout  «  "currentCameraPosition  =" 

«  "  <"  «  currentCameraPosition  [0] 

«  ",  "  «  currentCameraPosition  [1] 

«  ",  "  «  currentCameraPosition  [2]  «  ">"  «  endl; 
cout  «  "orientationRotation  =" 

«  "  <"  «  orientationRotationAxis  [0] 

«  ",  "  «  orientationRotationAxis  [1] 

«  ",  "  «  orientationRotationAxis  [2] 

«  ",  "  «  degrees  (orientationRotationAngle)  «  «  endl; 

*/ 

//  camera  control  complete 

} 

//  cout  «  "viewer:  end  of  DIS_Redraw_Callback  ()"  «  endl; 

return; 

} 


//  called  on  an  exit  condition  via  a  call  to  atexit  (DIS_net_close)  in  main 

static  void  DIS_net__close  () 

{ 

cout  «  "viewer:  DIS_net_close  {);"  «  endl; 
net_close  (); 

DIS_j)ort_open  =  FALSE; 

} 

- - - 


/ //////////////////////////////////////////////////////////////////////////// 
// 

//  This  is  called  by  the  Color  Editor  whenever  the  color 
//  has  changed.  The  userData  is  set  by  main()  in  the  call 
//  to  SoXtColorEditor: :addColorChangedCallback. 

void 

colorEditorCB (  void  ‘userData,  const  SbColor  ‘rgbCallbackData  ) 

SoXtRenderArea  ‘renderArea  =  (SoXtRenderArea  *)  userData; 
renderArea->setBackgroundColor(  ‘rgbCallbackData  ); 


///////////////////////////////////////////////////////////////////////////// 

SoSeparator  *  readPile (const  char  ‘filename)  //  Inventor  Mentor  p.  284 

//  Open  the  input  file 

Soinput  myScenelnput; 

if  ( imySceneInput.openFile (filename) ) 

{ 

cout  «  "Cannot  open  file  "  «  filename  «  endl; 
return  NULL; 

} 

//  Read  the  whole  file  into  the  database 
SoSeparator  ‘  myGraph  =  SoDB: : readAll (&mySceneInput) ; 
if  (myGraph  ==  NULL) 

{ 

cout  «  "Problem  reading  file  "  «  filename  «  endl; 
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return  NULL; 

} 

mySceneInput .closeFile  ( )  ; 
return  myGraph; 


///////////////////////////////////////////////////////////////////////////// 

void  initialize_globals  {) 

{ 

//  multicast  port  &  group 
bzero  (port,  6); 
strncpy  (port,  "3111",  4); 
bzero  (group,  30); 

strncpy  (group,  "224.2.244.141",  13);  //GROUP 

//  strncpy  (group,  "224.2.237.170",  13);  //  GROUP 

//  3111  is  npsnet  "standard'  port 
//  #define  DEFAULT_PORT  =  "3111"; 

//  #define  DEFAULT_GROUP  =  "224.2.121.93"; 

//  initialize  materials 

silver->  ambientColor . setValue  (  .2,  .2,  .2  ); 

silver->  diffuseColor.setValue  (  .6,  .6,  .6  ); 

silver’->specularColor. setValue  {  .5,  .5,  .5  ); 
silver->shininess  =  .5; 

gold->  ambientColor. setValue  {  .4,  .2,  .0  ); 

gold->  diffuseColor.setValue  (  .9,  .5,  .0  ); 

gold->specularColor. setValue  (  .7,  .7,  .0  ); 

gold->shininess  =:  .6; 

brass->  ambientColor. setValue  (  0.329412,  0.223529,  0.027451  j; 
brass->  diffuseColor.setValue  (  0.780392,  0.568627,  0.113725  ); 
brass->specularColor. setValue  (  0.992157,  0.941176,  0.807843  ); 
brass->shininess  =  0.21794872; 

chrome->  ambientColor . setValue  (  0.25,  0.25,  0.25  ); 

chrome“>  diffuseColor.setValue  {  0.4,  0.4,  0.4  ); 

chrome”>specularColor. setValue  (  0.774597,  0.774597,  0.774597  ); 
chrome->shininess  =  0.6; 

npsblue“>  ambientColor .setValue  (  0.0,  0.0,  1.0  ); 
npsblue->  diffuseColor.setValue  (  0.0,  0.0,  0.8  ); 
npsblue->specularColor. setValue  {  0.0,  0.2,  1.0  ); 
npsblue->shininess  =  0.8; 

seagreen->  ambientColor . setValue  {  0.0,  0.5,  0.0  ); 

seagreen~>  diffuseColor.setValue  (  0.0,  0.5,  0.0  ); 

seagreen->specularColor. setValue  (  0.0,  0.5,  0.0  ); 
seagreen->shininess  =0.0; 

seasurface->  ambientColor . setValue  (  0.3,  0.7,  0.5  ); 
seasurface->  diffuseColor.setValue  (  0.3,  0.7,  0.5  ); 
seasurface->specularColor. setValue  (  0.3,  0.7,  0.5  ); 
seasurface->shininess  =  0.0; 

seacolor~>  ambientColor . setValue  {  0.1,  0.1,  0.5  ); 

seacolor“>  diffuseColor.setValue  (  0.1,  0.1,  0.5  ); 

seacolor->specularColor. setValue  (  0.1,  0.1,  0.5  ); 
seacolor->shininess  =  0.0; 

darkgreen~>  ambientColor .setValue  (  0.15,  0.20,  0.15  ); 
darkgreen->  diffuseColor.setValue  (  0.15,  0.20,  0.15  ); 
darkgreen>>specularColor .setValue  (  0.15,  0.20,  0.15  ); 
darkgreen-“>shininess  =  0.0; 

sonar_cone_color72 5->  ambientColor . setValue  {  0.8,  0.0,  0.8  ); 
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sonar_cone_color725->  diffuseColor.setValue  {  0.8,  0.0,  0.8  ); 
sonar_cone_color725->specularColor.setValue  (  0.8,  0.0,  0.8  ) ; 
sonar_cone_color725“>shininess  =  0.0; 

sonar_cone_colorl000->  ambientColor . setValue  {  1.0,  0.0,  0.15  ); 
sonar_cone_colorl000->  diffuseColor.setValuG  (  1.0,  0.0,  0.15  ); 
sonar_cone_colorl000->specularColor.setValue  {  1.0,  0.0,  0.15  ); 
sonar_cone_colorl000->shininess  =0.0; 

wires->style  =  SoDrawStyle: : LINES; 

//  Units  of  distance  initialized 

MetersToFeet->scaleFactor. setValue  (  METERS__PER_FT ,  METERS_PER_FT, 

METERS_PER_FT  ) ; 

InchesToFeet->scaleFactor. setValue  {  12.0/1.0,  12.0/1.0,  12.0/1.0  ); 
FeetToInches~>scaleFactor. setValue  (  1.0/12.0,  1.0/12.0,  1.0/12.0  );- 

MinGFieldTransfonn->translat ion. setValue (  3280.8/2.0,  -3280 . 8/2 .0,  0.0); 
MineFieldScale->scaleFactor. setValue  (  3280.8/400.0,  3280.8/400,0,  1.0); 


//  initial  position  of  AUV 


AUV_x  =  5.0; 

AUV_/  =  5.0; 

AUV_2  =  2.0; 

AUV_ST1000_bearing  =  0; 

AUV_ST1000_range  =  -0.001; 

AUV_ST1000_strength  =  0; 

AUV_ST7-25_bearing  =  0; 

AUV__ST725_range  =  -0.001; 

AUV_ST725_strength  =  0; 


wakeCoinplexity->value  =  0.2; 
ST1000Co]:nplexity“>value  =  ST1000Complexity__off; 
ST725Complexity->value  =  ST725Coinplexity_of  f  ; 

return; 

}  //  end  initialize_globals  () 


///////////////////////////////////////////////////////////////////////////// 

///////////////////////////////////////////////////////////////////////////// 


void  parse_coiranand_line_f lags  (int  argc,  char  **  argv) 

//  command  line  arguments 


int  i; 


//  cout  «  "  [pars e_coinmand_line_f lags  start:  #  arguments  =  "  «  argc  «  "]" 
//  «  endl; 

//  cout  «  " [  "  ; 

//  for  (i  =  0;  i  <  argc;  i++)  cout  «  argv  [i]  «  "  " ; 

//  cout  «  "]"  «  endl; 


for  (i  =  1;  i  < 
{ 

if  ( (strcmp 

argc;  i++) 

(argv[i-l] , 

"CIRCLE-FILE")  != 

0) 

ScSc 

(strcmp 

{argv[i-l] , 

"CIRCLEFILE")  != 

0) 

ScSc 

(strcmp 

(argv[i-l] , 

"CIRCLE")  != 

0) 

ScSc 

(strcmp 

(argv[i-l] , 

"CYLINDER-FILE") != 

0) 

ScSc 

(strcmp 

(argv[i-l] , 

"CYLINDERFILE")  != 

0) 

ScSc 

(strcmp 

{argv[i-l] , 

"CYLINDER")  != 

0) 

ScSc 

(strcmp 

(argv[i-l] , 

"WORLD-FILE")  != 

0) 

ScSc 

(strcmp 

{argv[i-l] , 

"WORLDFILE")  1= 

0) 

ScSc 

(strcmp 

(argv[i-l] , 

"WORLD" )  1 = 

0); 

) 

for  (int  index  =  0;  index  <=  strlen  (argv[i]);  index++) 

argv[i]  [index]  =  toupper  (argv[i]  [index]); 


//  uppercase 


if  (  (strcmp  (argv[i],  "PORT"')  ==  0)  M 
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(strcmp  (argv[i],  "-PORT")  ==  0)  I  I 

(strcmp  (argviij,  "P")  ==  0)  II 

(strcmp  {argviij,  "-P")  ==  0)) 

{ 

if  {  i+1  >=  argc  ) 

cout  «  "Insufficient  parameters  for  PORT"  «  endl; 

else 

{ 

cout  «  "["  «  argv[i]  «  "  "  «  argv[i+l]  «  "]"  «  endl  ; 

strcpy  (port,  argv[i+l])  ; 

i++; 

} 

} 

else  if  ((strcmp  (argv[i],  "GROUP")  ==  0)  II 

(strcmp  {argv[i],  "-GROUP")  ==  0)  M 

(strcmp  (argviij,  "G")  ==  0)  11 

(strcmp  (argviij,  "-G")  ==  0)  11 

(strcmp  (argviij,  "ADDRESS")  ==  0)  11 

(strcmp  (argv[ij,  "-ADDRESS")  ==  0)  II 

(strcmp  (argviij,  "A")  ==  0)  M 

(strcmp  (argv[i],  "-A")  ==  0)) 

{ 

if  (  i+1  >=  argc  ) 

cout  «  "Insufficient  parameters  for  GROUP  ADDRESS"  «  endl;’ 

else 

{ 

cout  «  "["  «  argv[i]  «  "  "  «  argv[i+l]  «  «  endl; 

strcpy  (group,  argv[,i  +  l]); 

i  +  + ; 

cout  «  "("  «  argv[i]  «  "  "  «  argv[i  +  l]  «  "j"  «  endl  ; 

} 

} 

else  if  ((strcmp  (argv[i] ,  "PRINTDIALOG" )  ==  0)  M 
(strcmp  (argv[i],  "-PRINTDIALOG")  ==  0)  N 
(strcmp  (argviij,  "PRINT")  ==0)  II 
(strcmp  (argv[i],  "-PRINT")  ==  0)) 

{ 

PRINTDIALOG  =  TRUE; 

cout  «  "["  «  argv[i]  «  "]"  «  endl; 

} 

else  if  ({strcmp  (argv[i] ,  "NOPRINTDIALOG" )  ==0)  II 
(strcmp  (argv[i],  "-NOPRINTDIALOG")  ==  0)  il 
(strcmp  (argviij,  "NOPRINT")  ==  0)  M 

(strcmp  {argv[i],  "-NOPRINT")  ==  0)) 

{ 

PRINTDIALOG  =  FALSE; 

cout  «  "["  «  argv[i]  «  "]"  «  endl; 

} 

else  if  ((strcmp  (argv[i],  "TEXTURE")  ==  0)  M 

(strcmp  (argv[i],  "-TEXTURE")  ==  0)  II 
(strcmp  (argv[i],  "TEXTURE-ON")  ==  0)  I  I 

(strcmp  (argviij,  "-TEXTURE- ON")  ==  0)  I  I 

(strcmp  (argviij,  "T"  )  ==  O)  11 

(strcmp  (argviij,  "-T"  )  ==  0)) 

{ 

TEXTURE  =  TRUE; 

cout  «  "["  «  argv(ij  «  "j"  «  endl; 

} 

else  if  ((strcmp  (argv[ij,  "MINEFIELD")  ==  0)  II 

(strcmp  (argviij,  "-MINE-FIELD")  ==  0)) 

MINEFIELD  =  TRUE;  //  special  color  scheme 

cout  «  "["  «  argviij  «  "j"  «  endl ; 

} 

else  if  ((strcmp  (argv[ij,  "NOTEXTURE")  ==  0)  II 

(strcmp  (argviij,  "-NOTEXTURE" )  ==  0)  M 

(strcmp  (argviij,  " TEXTURE- OFF " )  ==  0)  j| 

(strcmp  (argviij,  " -TEXTURE- OFF" )  ==  0)  II 
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(strcmp 

(argv[i] , 

"NO-TEXTURE"  ] 

1  rrz: 

0) 

( 

(strcmp 

(argv[i] , 

"-NO- TEXTURE") 

1  =  = 

0)) 

TEXTURE  = 

FALSE; 

} 

else 

cout  «  " 1 

«  argv[i]  «  "j"  « 

endl  ; 

if  ( (strcmp 

(argv[i] , 

"SCREENDOOR") 

== 

0) 

(strcmp 

(argv[i] , 

"-SCREENDOOR") 

== 

0) 

(strcmp 

(argv[i] , 

"SCREEN-DOOR'- 

^)  == 

0) 

{ 

(strcmp 

{argv[i] , 

"-SCREEN-DOOR" 

')  == 

0)) 

SCREENDOOR  =  TRUE; 

cout  «  " [ 

"  «  argvl 

[i]  «  "]"  « 

endl  ; 

/ 

else 

if  ( (strcmp 

(argv[i] , 

"LIGHTSOUT") 

=  =  0 

)  1 

(strcmp 

(argv[i] , 

"-LIGHTSOUT" ) 

0)  1 

(strcmp 

(argv[i]  , 

"LIGHTS-OUT") 

=  =  0 

)  I 

(strcmp 

(argvdJ  , 

"-LIGHTS-OUT") 

==  0)  1 

(strcmp 

(argv[i]  , 

"NOLIGHTS")  = 

:=  0) 

I  i 

(strcmp 

(argv(i] , 

" -NOLIGHTS")  = 

=  0) 

1  1 

(strcmp 

(argv[i]  , 

"NO-LIGHTS") 

=  =  0) 

1  1 

{ 

(strcmp 

(argv(i] , 

"-NO-LIGHTS") 

=  =  0) 

) 

LIGHTSOUT 

=  TRUE; 

} 

else 

cout  «  " [ 

"  «  argv[i]  «  "]"  « 

endl  ; 

if  { (strcmp 

(argv[i] , 

"FILE")  ==  0) 

1  1 

(strcmp 

(argv[i] , 

"-FILE")  ==  0) 

1  1 

(strcmp 

(argv[i] , 

"F")  ==  0) 

1  1 

(strcmp 

(argv[i] , 

"-F")  ==  0)) 

{ 


if  (  i+1  >=  argc  ) 

cout  «  "Insufficient  parameters  for  FILE  filename. iv" 

«  endl ; 

else 

{ 

cout  «  «  argv[i]  «  "  "  «  argv[i+l]  «  «  endl 

command_line_node  =  new  SoSeparator; 
command_line_node  =  readFile  (argv[i+l]); 
root“>  addChild  (  command_line_node  ) ; 

.  i++; 

} 

} 

else  if  ( (strcmp  (argv[i],  "BACKGROUNDCOLORDIALOG" )  ==  0)  11 

(strcmp  {argv[i],  "-BACKGROUNDCOLORDIALOG" )  ==  0)) 

{ 

BACKGROUNDCOLORDIALOG  =  TRUE; 

cout  «  "["  «  argvfi]  «  "]"  «  endl; 

} 

else  if  ((strcmp  (argv[i] ,  "NOBACKGROUNDCOLORDIALOG" )  ==0)  II 
(strcmp  (argv[i],  "-NOBACKGROUNDCOLORDIALOG" )  ==  0)) 

{ 

BACKGROUNDCOLORDIALOG  =  FALSE; 

cout  «  "["  «  argv[i]  «  «  endl; 

} 

else  if  ({strcmp  (argv[i],  "DEADRECKON")  ==  0)  II 
(strcmp  (argv[i],  "-DEADRECKON")  ==  0)  I  I 
(strcmp  (argv[i],  "DR")  ==  0)  II 

(strcmp  (argv[i],  "-DR")  ==  0)) 

{ 

DEADRECKON  =  TRUE; 

cout  «  «  argv[i]  «  "]"  «  endl; 

} 

else  if  ((strcmp  {argv[i],  "NODEADRECKON" )  ==0)  M 

(strcmp  (argvii],  "-NODEADRECKON")  ==  0)  M 
(strcmp  (argv[i],  " NO- DEADRECKON" )  ==  0)  M 
(strcmp  (argv[i],  "-NO-DEADRECKON" )  ==  0)  11 

(strcmp  (argv[i],  "NODR")  ==0)  ll 
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(strcmp 

(argv[i] , 

"-NODR") 

== 

0)  [  i 

(strcmp 

(argv[i] , 

"NO-DR" ) 

== 

0)  1  1 

(strcmp 

(argv[i] , 

" -NO-DR") 

0)  I  ! 

(strcmp 

(argv[i] , 

"NO  DR") 

=  = 

0)  1  I 

(strcmp 

(argv[i] , 

"-NO  DR") 

=  = 

0)) 

DEADRECKON 

=  FALSE; 

cout  «  " [ 

"  «  argv[il  «  "] 

" . «  endl ; 

if  ( (strcmp 

{argv[i] , 

"CAMERA" 

)  ==  0)  II 

(strcmp 

(argv[i] , 

"-CAMERA" 

)  ==  0)) 

cout  « 

«  argv[l] 

«  "  " 

«  argv[i+l] 

/ 

if  (i+1  >= 

argc)  // 

trying  to 

look  past  command 

} 

else  if 


whichCamera  = 
cout  «  "  (no 
«  endl; 


CAMERA^FREE; 
camera  specified. 


CAMERA^FREE  used)]" 


( (strcmp 
(strcrap 
(strcmp 
(strcmp 
(strcmp 


whichCamera 
cout  «  ' 


(argv[i+l] , 
{argv[i+l] , 
(argv[i+l] , 
(argv[i+l ] , 
(argv[i  +  l] , 


"1") 

"CAMERA_FREE" ) 
"camera_free" ) 
"FREE") 

"free") 


0) 

0) 

0) 

0) 

0)) 


} 

else 


=  CAMERA^FREE; 
(CAMERA_FREE)  ; 


«  endl; 


} 

else 


if 

(‘{strcmp 

(argv[i+l] , 

"2") 

== 

0) 

(strcmp 

(argv[i+l) , 

"CAMERA_TO_AUV" ) 

0) 

(strcmp 

(argv[i+l) , 

"camera  to  auv" ) 

== 

0) 

(strcmp 

(argv[i+l] , 

"TO_AUV" ) 

0) 

( s  t  rcmp 

(argv[l+i] , 

" to_auv" ) 

=:  = 

0) 

(strcmp 

(argv[i+l] , 

"CAMERA-TO-AUV" ) 

=  = 

0) 

(strcmp 

(argv[i+l] , 

"camera -to- auv" ) 

=  = 

0) 

(strcmp 

{argv[i+l) , 

"TO-AUV") 

=  = 

0) 

(strcmp 

(argv[i+l] , 

"to-auv") 

=  = 

0)) 

whichCamera 

=  CAMERA  TO 

_AUV; 

cout  «  " 

(CAMERA_TO. 

^AUV)]"  «  endl; 

if 

( (strcmp 

(argv[i+l] , 

"3") 

r 

:=  0 

(strcmp 

(strcmp 

(strcmp 

(strcmp 

(strcmp 

(strcmp 

(strcmp 

(strcmp 

whichCamera 

cout  « 


{argv[i+l] , 
(argv[i+l] , 
(argv[i+l] , 
(argv[i+l] , 
(argv[i+l] , 
(argv[i+l] , 
(argv[i+l] , 
(argv[i+l] , 


"CAMERA_FRO]yLAUV" ) 
"  c  ame  r  a_f  rom_au  v" ) 
"FROM_AUV") 

" f rom_auv" ) 
"CAMERA-FROM-AUV" ) 
"  c  ame  r  a  -  f  r  om-  au  v" ) 
"FROM-AUV") 
"from-auv") 


0) 

0) 

0) 

0) 

0) 

0) 

0) 

0)) 


=  CAMERA_FROM_AW; 
{CAMERA_FROM_AUV)  ]  ' 


«  endl; 


else 

{ 


} 

else 


} 

i  +  +  ; 


whichCamera  =  CAMERA_TO_AUV ; 
cout  «"  (unknown  option  ignored, 

//  skip  camera  parameter 


CAMERA_TO_AUV  used)  ]  "  «  endl 


if  ((strcmp  (argv[i],  "TESTTANKSURFACE" )  ==  0) 

(strcmp  (argv[i],  "-TESTTANKSURFACE" )  ==  0) 

(strcmp  (argv[i],  "TESTTANK-SURFACE" )  ==  0) 
(strcmp  (argv[i],  "-TESTTANK- SURFACE")  ==  0) 
(strcmp  (argv[i],  "TEST-TANK- SURFACE" ) ==  0) 
(strcmp  (argv[i],  "-TEST-TANK-SURFACE" ) ==  0) 
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(strcmp  {argv[i],  "SURFACE")  ==  0)  M 

(strcmp  (argv[i],  "-SURFACE")  ==  0)) 

TESTTANKSURFACE  =  TRUE; 

cout  «  "["  «  argv[i]  «  "]"  «  endl; 


(strcmp 

(argv[i] , 

"CIRCLE-FILE") 

=  =: 

0) 

(strcmp 

(argv[i] , 

"CIRCLEFILE") 

=  = 

0) 

(strcmp 

(argv[i] , 

"CIRCLE") 

=  = 

0) 

( strcmp 

{argv[i) , 

"CYLINDERFILE") 

=  = 

0) 

(strcmp 

(argv[i] , 

"CYLINDER-FILE" 

0) 

(strcmp 

(argv[i] , 

"CYLINDER") 

=  = 

0) 

) 

if  (READCIRCLEFILE) 


cout  «  "Error  -  cannot  open  more  than  one  circle  file:  " 
«  argv[i+l]  «  endl; 
exit  (-1); 

} 

if  (i  +  1  >=  argc) 

cout  «  "Insufficient  parameters  for  CIRCLE-FILE"  «  endl; 

else 

{ 


i++; 

char  newfilename  [50]; 
s t  rcpy  ( newf i 1 ename ,  argv [ i ] ) ; 
circleFile. open (newfilename, ios: :in) ; 
if  ( I circleFile) 

{ 

strcpy  (newfilename,  "../tactical/"); 
strcat  (newfilename,  argv[i]); 
circleFile. open (newfilename, ios : :in) ; 

} 

if  ( icircleFile) 

{ 


cout  «  "Error  opening  circle  file:  "  «  argv[i]  «  endl; 
exit  (-1) ; 

} 

else 

{ 

cout  «  " [Circle- File  "  «  newfilename  «  "  opened]" 

«  endl; 

READCIRCLEFILE  =  TRUE; 

} 


} 

else  if  ((strcmp  .(argv[i],  "WORLD-FILE")  == 

(strcmp  (argv[i],  "WORLDFILE")  == 

(strcmp  (argvii],  "WORLD")  == 

{ 

if  (i  +  1  >=  argc) 

cout  «  "Insufficient  parameters 

else 

{ 


0)  I  I 
0)  i  i 
0)) 


for  WORLD-FILE"  «  endl; 


i  +  +  ; 

SoSeparator  *  worldSeparator  =  readFile  (argv[i]); 
if  (! worldSeparator) 

{ 

char  newfilename  [50]; 
strcpy  (newfilename,  "../viewer/"); 
strcat  (newfilename,  argv[i]); 
worldSeparator  =  readFile  (newfilename) ; 

} 

i f  (worldSeparator ) 

{ 


scene ry->addChi Id  (worldSeparator) ; 

cout  «  "[World  File  "  «  argv[i]  «  "  opened]"  «  endl; 
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else 

{ 

cout  «  "Unable  to  locate  world-file  "  «  argv[i]  «  cout 
exit  (-1); 

} 

} 

}. 

else  cout  «  "Unrecognized  command  line  parameter:  «  argv[i] 

«  ignored."  «  endl; 

}  //  end  for  loop  through  command  line  parameters 

//  cout  «  " [par se_command_line_f lags  complete]"  «  endl; 

return; 

}  //  end  parse_command_line_f lags  () 

///////////////////////////////////////////////////////////////////////////// 

//  Keypress  event  callbacks:  Inventor  Mentor  p.  464-465 
/* 

void  key_C_PressCB  (void  SoAction  *) 

{ 

//  cout  «  "key_C_PressCB  entered,  key_event->getKey  ()  =  " 

//  «  key_event->getKey  {)  «  endl; 

//  Check  for  C  key  being  pressed: 
if  {key_event->getKey  ()  ==  SoKeyboardEvent: :C) 

{ 

whichCamera  =  (whichCamera++ )  %  3; 

cout  «  "key_C_PressCB  camera  toggled  to  "  «  whichCamera  «  endl; 

} 

}  //  key_C_PressCB  complete 

//  Keypress  event  callbacks:  Inventor  Mentor  p.  266-267,  SceneVi ewer .C++ 
SbBool  key_C_PressCB  (void  *  userData,  XAnyEvent  *  event) 

SbBool  handled  =  FALSE; 

//  SoXtRenderArea  *  ityRenderArea  =  { SoXtRenderArea  *)  userData; 

cout  «  "key_C_PressCB  entered"  «  endl; 

//  Check  for  C  key  being  pressed: 

if  (event->type  ==  Key Press) 

{ 

if  (XLookupKeysym  ( (XKey Event  *)  event,  0)  ==  XK_C) 

{ 

whichCamera  =  (whichCamera++ )  %  3; 

cout  «  "key_C_PressCB  camera  toggled  to  "  «  whichCamera  «  endl; 
handled  =  TRUE; 

} 

) 

return  handled; 

}  //  key_C_PressCB  complete 
*/ 


///////////////////////////////////////////////////////////////////////////// 

///////////////////////////////////////////////////////////////////////////// 

void  main  (  int  argc,  char  **  argv  ) 

{ 

//  Initialize  Inventor  and  Xt  -  these  steps  MUST  be  first  calls 
//in  main,  without  exception,  or  a  mystery  crash  results. 

Widget  ViewerWindowWidget  =  SoXt : : init (argv[0] ) ; 
if  (  ViewerWindowWidget  ==  NULL  ) 
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{ 

cout  «  "viewer:  ViewerWindowWidget  ==  NULL  on  startup,  exiting." 

«  endl; 
exit(  1  ); 

} 

cout  «  "viewer:  ViewerWindowWidget  added"  «  endl; 

initialize_globals  ( ) ; 

parse_command_line_f lags  {argc,  argv) ; 

//  port  and  group  can  change  by  coirmiand  line  switches 
cout  «  "multicast  port  =  "  «  port 

«  ",  address  group  =  "  «  group  «  endl; 

cout  «  "creating  the  scene  graph:  "  ; 

root  =  new  SoSeparator; 
root-“>ref  { )  ; 

cout  «  "root  added"  «  endl; 

//  Keypress  event  callbacks:  Inventor  Mentor  p.  265-267,  class  notes  ch.  10 
//  SoEventCallback  *key_C__EventCB  =  new  SoEventCallback; 

//  k€y_C_EventCB->addEventCallback  (SoKeyboardEvent : :getClassIVpeId() , 

//  key_C_PressCB,  root); 

//  root->addChild  (key_C_EventCB) ; 

//  Keypress  event  callbacks:  Inventor  Mentor  p.  465 
//  SoCallback  *key_C_EventCB  =  new  SoCallback  {); 

//  key_C_EventCB->setCallback  (key_C_PressCB)  ; 

//  root->addChild  (key_C_EventCB)  ; 

//  correct  for  different  coordinate  system  -  not  yet  needed 
//  SoRotationXYZ  *  coordinateSystemFlip  =  new  SoRotationXYZ; 

//  coordinateSystemFlip->angle .setValue  (  M_PI  ); 

//  coordinateSystemFlip->  axis . setValue  (  SoRotationXYZ : :X  ); 

//  root->addChild (  coordinateSystemFlip  ); 

SoRotationXYZ  *ro0x  =  new  SoRotationXYZ; 

roOx->angle. setValue  (  0.0  ); 

ro0x->  axis. setValue  (SoRotationXYZ : :X) ; 

SoRotationXYZ  *ro90x  =  new  SoRotationXYZ; 
ro90x->angle. setValue  (M_PI  /  2.0); 
ro90x->  axis. setValue  (SoRotationXYZ : :X) ; 

SoRotationXYZ  *ro270x  =  new  SoRotationXYZ; 
ro270x->angle. setValue  (3.0  *  M_PI  /  2.0); 
ro270X“>  axis. setValue  (SoRo.tationXYZ:  :X)  ; 

PerspectiveGameraFree  =  new  SoPerspectiveCamera; 

PerspectiveCameraToAUV  =  new  SoPerspectiveCamera; 
PerspectiveCameraFromAUV  =  new  SoPerspectiveCamera; 

//  can't  put  a  group  or  separator  above  camera 

//  cameras  using  pointAt  are  90  degrees  twisted  askew 

//  Create  a  camera  SoSwitch  node 

SoSwitch  *  whichCamer a Switch  =  new  SoSwitch; 

root->addChild  {  whichCamera Switch  ); 

whichCameraSwitch->addChild  (  PerspectiveGameraFree  ) ; 
whichCameraSwitch->addChild  (  PerspectiveCameraToAUV  )  ; 
whichCameraSwitch->addChild  {  PerspectiveCameraFromAUV  ) ; 

.//  select  camera 

whichCameraSwitch->whichChild  =  whichCamera; 

//  Create  a  camera  rotation  correction  SoSwitch  node 
SoSwitch  *  whichCameraCorrectionSwitch  =  new  SoSwitch; 
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root->addChild  (  whichCameraCorrectionSwitch  ) ; 
whichCameraCorrectionSwitch“>addChild  (  roOx  ) ; 
whichCaineraCorrectionSwitch'->addChild  (  ro270x  ); 
whichCameraCorrectionSwitch“>addChild  (  ro270x  ); 
whichCaineraCorrectionSwitch->whichChild  =  whichCamera; 

SoXtRenderArea  *  myRenderArea  =  new  SoXtRenderArea  (ViewerWindowWidget )  ; 

//  Keypress  event  callbacks:  Inventor  Mentor  p.  266-267 
//  myRenderArea->setEventCallback  {key_C_PressCB,  myRenderArea); 

myRegion  =  new  SbViewportRegion  (myRenderArea- >get Size  {)); 

if  (SCREENDOOR  ==  TRUE) 

myRenderArea ->setTransparencyType  (SoGLRenderAction:  :  SCREE ISLDOOR) ; 

else 

inyRenderArea->setTransparencyType  {SoGLRenderAc¬ 
tion:  :SORTED_OBJECT_BLEND) ; 


if  (LIGHTSOUT  ==  FALSE) 

root->addChild (  new  SoPointLight  ); 


//  2.0  ivview/ivquicken  .fails  on  SoUnits  nodes  so  SoScale  nodes  used  instead 
unitsfeet  =  new  SoUnits; 

unitsfeet->units  .  setValue  (  SoUnits ::  f'eIet  ); 
root->addChild {  unitsfeet  ); 

//  root->addChild (  MetersToFeet  ); 


currentAUVPosi tion  =  SbVec3f 

aheadOfAUVPosi tion  =  SbVec3f 

behindAUVPosition  =  SbVec3f 


{  AUV_x  *  METERS_PER_FT, 
-AUV_2  *  METERS_PER_FT, 
AUV_y  *  METERS_PER_FT  )  ; 


(  5.0  *  sin 

2.0  *  sin 
5.0  *  cos 
(  -5,0  *  sin 
“2.0  *  sin 
-3.0  *  sin 
-5.0  *  cos 


(  AUV_psi  ) 
(  AUV^theta  ) 
(  AUV_psi  ) 
(  AUV_psi  ) 
(  AUV_theta  ) 
(  AUV_thGta  ) 
{  AUV_^si  ) 


+  0.5)  ; 

+  0.5)  ; 


//  Free  (unmodified)  Camera 

PerspectiveCameraFree->viewAll  (root,  *myRegion,  1.0);  //  global 

PerspectiveCameraFree->aspectRatio . setValue  (SO_ASPECT_yiDEO) ; 


//  Camera  that  keeps  AUV  in  center 

PerspectiveCameraToAUV->viewAll  (root,  *ityRegion,  1.0);  //  global 

PerspectiveCameraToAUV->aspectRatio. setValue  (SO_ASPECT_VIDEO)  ; 
PerspectiveCameraToAUV->posi tion. setValue 

(  currentAUVPosition  +  standardCameraOf f set  ) ; 
/ /  PerspectivGCaraeraToAUV->oriGntation . setValue 

//  {  SbRotation  (1.0,  0.0,  0.0,  M_PI  /  2,0  )  ); 

PerspectiveCameraToAUV->pointAt  {  currentAUVPosition  ) ; 


//  Camera  that  looks  out  from  AUV  in  center 

PerspectiveCameraFromAUV->viewAll  (root,  *myRegion,  1.0);  //  global 

PerspectiveCameraFromAUV->aspectRatio. setValue  (SO_ASPECT_VIDEO)  ; 
PerspectiveCameraFromAUV->posi tion. setValue  (  currentAUVPosition 

+AUVCameraOf  f set 
+behindAUVPosition  ) ; 

//  PerspectivGCameraFromAUV->orientation. setValue 

//  (SbRotation  (1.0,  0.0,  0.0,  M_PI  /  2.0  )); 

PerspectiveCameraFromAUV->pointAt  (  currentAUVPosition 

+AUVCameraOf f set 
+ aheadOfAUVPosi tion  ) ; 


SoPickStyle  *  unpickablestylenode; 
SoPickStyle  *  pickablestylenode; 
unpickablestylenode  =  new  SoPickStyle; 
pickablestylenode  =  new  SoPickStyle; 
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unpickablestylenode->style.setValue  {  SoPickStyle: :UNPICKABLE  ); 
pickablestylenode->style.SGtValue  (  SoPickStyle: : SHAPE  ); 

//  create  the  terrain  box 
SoCube  *  backgroundCube  =  new  SoCube; 
backgroundCube~>width  =  400.0; 
backgroundCube-*>height  =  4  00.0; 
backgroundCube->depth  =  1.0; 

SoTransform  *  xf backgroundCube  =  new  SoTransform; 
xf backgroundCube->translation . setValue (0 . 0 ,  0.0,  -50.0); 

SoTexture2  *  overhangTexture  =  new  SoTexture2; 
overhangTexture->filename. setValue  { "overhang. rgb" ) ; 

SoSeparator  *  sepbackgroundCube  =  new  SoSeparator; 
sepbackgroundCube->addChild(  pickablestylenode  ) ; 
sepbackgroundCube- >addChild{  unitsfeet  ) ; 
if  (TEXTURE  ==  TRUE) 

sepbackgroundCube->addChild(  overhangTexture  ); 
sepbackgroundCube- >addChi Id (  xfbackgroundCube  ); 
if  (MINEFIELD) 

{ 

sepbackgroundCube->addChild(  seacolor  ); 
sepbackgroundCube->addChild(  MineFieldTransfonn  ); 
sepbackgroundCube->addChild (  MineFieldScale  ); 

} 

else  sepbackgroundCube->addChild(  darkgreen  ); 

sepbackgroundCube->addChild(  backgroundCube  ) ;  //  remove  cube  here 

root->addChild  (  sepbackgroundCube  ); 

root->addChild  (  scenery  );  //  pickable  objects  go  in  this  separator 

//  Read  from  circleFile  and  draw  cylinders  (if  required) 
if  (READCIRCLEFILE) 
readCircles  (); 

//  Jason  with  engine  animation 
SoSeparator  *  sepJason  =  new  SoSeparator; 

SoTransform  *  xfJason  =  new  SoTransform; 

SoSeparator  *  Jason  =  readFile  ("Jason.iv"); 

xfJason->translation. setValue (  -60.0,  40.0,  -  25.0);  //  center  of  pattern 

sepJason->addChild  (  xf Jason  ) ; 

If  animation  from  Inventor  Mentor  13 . 6 .Calculator . C++ 

//  Set  up  the  Jason  transformations 
SoRotationXYZ  *  danceRotate  =  new  SoRotationXYZ; 
danceRotate->angle. setValue  (  0.0  ); 
danceRotate->  axis . setValue  (  SoRotationXYZ: : Z  ); 
sepJason->addChild  (  danceRotate  ) ; 

SoTranslation  *  danceTranslate  =  new  SoTranslation; 
sepJason->addChild  (  danceTranslate  ) ; 
sepJason->addChild  (  Jason  ) ; 
root->  addChild  (  sepJason  ) ; 

//  Set  up  an  engine  to  calculate  the  motion  path: 

//  Theta  is  incremented  using  a  time  counter  engine, 

//  and  converted  to  radians  using  an  expression  in 
//  the  calculator  engine. 

SoCalculator  *  calcXY  =  new  SoCalculator; 

SoTimeCounter  *  thetaCounter  =  new  SoTimeCounter; 
thetaCounter->max  =360; 
thetaCounter->step  =  1; 

thetaCounter->frequency  =  1  /  180.0;  //  180  seconds  for  a  full  cycle 
calcXy->a.connectFrom(ScthetaCounter->output)  ; 

calcXY->expressi on. set lvalue (0,  "ta=a*M_PI/180") ;  //  theta  (radians)  * 

calcXY->expression.setlValuG (1,  "tb=15*cos (ta) " ) ;  //  r,  z 

calcXY->expression. set lvalue (2,  "td=tb*cos ( ta) " ) ;  //  x 

calcXY->expression.setlValue (3 ,  "te=tb*sin (ta) " ) ;  //  y 

calcXY->expression.setlValue (4,  "oA=vec3f ( td, te, tb) " ) ;  //  vector  output  A 
danceTranslate->translation. connectFrom (&calcXY->oA) ; 

calcXY->expression . setlValue (5 ,  "ob=2*ta");  //  scalar  output  b 
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danceRotate“>angle . connectFrom (SccalcXY->ob)  ; 

//  Oil  Platform 

SoSeparator  *  sepPlatform  =  new  SoSeparator; 

SoTransform  *  xfPlatform  =  new  SoTransform; 

SoSeparator  *  Platform  =  readFile  ("Platform.iv"); 
xfPlatform“>translation. setValue (-80.0,  50.0,  -50.0); 
sepPlatform- >addChild  (  xfPlatform  ) ; 
sepPlatf orm->addChild(  Platform  ); 
scenery- >addChi Id  (  sepPlatform  ) ; 

//  Testtank 

SoSeparator  *  sepTesttank  =  new  SoSeparator; 

SoTransform  *  xfTesttank  =  new  SoTransform; 

SoSeparator  *  Testtank  =  readFile  ("testtank. iv") ; 
if  (TESTTANKSURFACE) 

xfTesttank->translation.setValue(  0.0,  0.0,  -6.0); 

else 

xfTesttank->translation.setValue(  0.0,  0,0,  -50.0) ; 
sepTesttank->addChild  (  xfTesttank  ) ; 
sepTesttank->addChild(  Testtank  ); 
scenerY->addChild  (  sepTesttank  ) ; 

//  Torpedo  Tube 

SoSeparator  *  sepTorpedoTube  =  new  SoSeparator; 

SoTransform  *  xfTorpedoTube  =  new  SoTransform; 
xfTorpedoTube->translation . setvalue (  30,0, 

SoCylinder  *  TorpedoTube  =  new  SoCylinder; 

TorpedoTube->radius  =21.0/12.0;  // 

TorpedoTube->height  =10.0;  // 

TorpedoTube->parts  =  SoCylinder :: SIDES;  // 
sepTorpedoTube->addChild  (  xfTorpedoTube  ) ; 
sepTorpedoTube ->addChi Id (  TorpedoTube  ); 
scenery- >addChild  (  brass  ) ; 
scenery->addChild  {  sepTorpedoTube  ) ; 

//  Initialize  for  makeAUV 
whichWakeFV->addChild  (topsideBow) ; 
whichWakeFV->addChild  (bottomsideBow) ; 
whichWakeFV->whichChild  =  0;  //  default  topsideBow 
whichWakeAV->addChild  (topsideStern) ; 
whichWakeAV->addChild  (bottomsideStern) ; 
whichWakeAV->whichChild  =  0;  //  default  topsideStern 
whichWakeFH->addChild  (lef tsideBow) ; 
whichWakeFH->addChild  (rightsideBow) ; 
whichWakeFH->whichChild  =0;  //  default  lef tsideBow 
whichWakeAH->addChild  (lef t sides tern) ; 
whichWakeAH->addChild  ( right sideS tern) ; 
whichWakeAH->whichChild  =  0;  //  default  lef tsideStern 

//  Now  get  the  AUV  using  makeAUV  (); 

SoSeparator  *  AUV_node  =  makeAUV  (); 

scenery- >addChi Id  (  AUV_node  );  //  AUV  object  creation  routine  above 

//  WriteAction  writes  scene  graph  to  file 
FILE  *  auv_iv_fp  =  fopen  ("auv.iv",  "w"); 

SoWriteAction  writeaction; 

writeaction.getOutput ( ) ->  setFilePointer  (auv_iv_fp) ; 
writeaction. apply  (AUV_node) ; 
f close  (auv__iv_fp)  ; 

cout  «  "writeaction. apply  (AUV_node)  =>  auv.iv  complete."  «  endl; 

//  WriteAction  writes  scene  graph  to  file 

FILE  *  auv_uvw_iv_fp  =  fopen  ( "auv_uvw. iv" ,  "w"); 

writeaction.getOutput 0 ->  setFilePointer  {auv__uvw_iv_fp) ; 

writeaction. apply  (root) ; 

f close  ( auv_uvw_i v_f p ) ; 

cout  «  "  writeaction. apply  (root)  =>  auv_uvw.iv  complete."  «  endl; 


20.0,  -48.00  ) ; 

21"  diameter 

10'  length 

let's  drive  through! 
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FILE  *  auv_wrl__fp  =  fopen  ("auv.wrl"/  "w"); 
writeaction, getOutput ( ) ->  setFilePointer  (auv_wrl_fp) ; 
writ:eaction.getOutput()->  setHeaderString  ("#VRML  Vl.O  ascii"); 
writeaction. apply  (AUV_node) ; 
f close  (auv_wrl_fp) ; 

cout  «  "writeaction. apply  {AUV_node)  =>  auv.wrl  complete."  «  endl; 

FILE  *  auv_uvw_wrl__fp  =  fopen  {"auv_uvw.wri",  "w"); 
writeaction. getOutput 0 ”>  setFilePointer  (auv_uvw_wrl_fp); 
writeaction. getOutput {)->  setHeaderString  ("#VRML  VI. 0  ascii"); 
writeaction. apply  (root); 
f close  (auv_uvw__wrl_fp)  ; 

cout  «  "  writeaction. apply  (root)  =>  auv_uvw.wri  complete."  «  endl ; 

system  ("ivfix  -Va  auv.iv  auv. f ix. iv" ) ; 

cout  «  "ivfix  -Va  auv.iv  auv. fix. iv  complete."  «  endl; 

DIS_net_open  (); 

atexit  (DIS_net_close) ;  //  ensure  port  is  reclosed  on  exit,  tested  sat. 

current_clock  =  clock  ();  //  initialize 

//  A  TimerSensor  updates  the  object  with  DIS  postures  and  performs  redraws 
SoTransfom  *  dummy_xform  =  new  So.Transform; 

SoTimerSensor  *  DIS_Redraw_Sensor  =  new 'soTimerSensor {  DIS_Redraw__Callback, 

duirany_xfonn  )  ; 

DIS_Redraw_Sensor->setInterval  {  0.10  );  //  seconds 

DIS_Redraw_Sensor->schedule  () ; 

//  system  ("rm  sounds/nps__auv.au" ) ; 

//  system  ("www  -o  sounds /nps_auv.au  f ile : //taurus . cs .nps .navy .mil /pub/auv/ 
nps_auv.au" )  ; 

//  system  ("www  -o  sounds /nps_auv. au  http://www_tios.cs.utwente.nl/say/ 

?Naval  +  Postgraduate+School , Autonomous+Underwater+Vehicle" )  ; 

//  system  ("sfplay  sounds /nps_auv.au  &"); 

//  Ocean  surface  (surf)  wiredraw  grid  rectangles  using  FaceSet  ------ 

//  follows  AUV  graphics  object  so  that  it  is  not  culled  from  scene 
const  int  N  =  10; 

//  type  changed  from  long  to  int32_t  for  Inventor  2.1  upgrade 

static  int32_t  numbersurfvertices  ((2*N+1)]  =  {4,  4,  4,  4,  4,  4,  4,  4,  4,  4, 
4,  4,  4/  4,  4,  4,  4,  4,  4,  4,  4};  //  ref.  p.  103 

//  2N+1  quadrilaterals,  3  coordinate  values  per  point 
static  float  surfquadvertices  [ (2*N+1 ) *4] [3 ] ; 
static  int  i  =  0; 
static  int  j  =  0; 


for  (i  =  1;  i  <=  N“lj 
{ 

surfquadvertices 

:  i  =  i  + 

2) 

// 

(box  pattern  black  one) 

[j  ][0] 

=  i; 

// 

X 

surfquadvertices 

[j  ][1] 

=  0; 

// 

y 

surfquadvertices 

[j  112] 

=  0; 

// 

z 

surfquadvertices 

[j+1]  [0] 

=  N; 

// 

X 

surfquadvertices 

[j+1]  [1] 

=  N-i; 

// 

Y 

surfquadvertices 

tj+1]  [2] 

=  0; 

// 

z 

surfquadvertices 

[j+2]  [0] 

=  N; 

// 

X 

surfquadvertices 

[j+2]  [1] 

=  N-i+1; 

// 

y 

surfquadvertices 

[j+2]  [2] 

=  0; 

// 

z 

surfquadvertices 

[j  +  3]  [0] 

=  i-1; 

// 

X 

surfquadvertices 

[ j  +  3]  [1] 

=  0; 

// 

y 

surfquadvertices 

[j+3] [2] 

=  0; 

// 

z 

j  =  j  +  4; 
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} 

if  (TRACE) 

cout  «  "surfquadvertices  block  1  complete,  j  range  =  [0,  "  «  j-1 
«  " ] "  «  endl ; 


int  old_j  =  j;  //do  not  modify  j,  it  will  continue 


(i  =  0;  i  <=  N-1; 

i  +=  2) 

// 

(box 

surfquadvertices 

[j  ]  [0] 

= 

0; 

// 

X 

surfquadvertices 

[j  ][1] 

= 

i; 

// 

y 

surfquadvertices 

[j  ][2] 

= 

0; 

// 

z 

surfquadvertices 

[j+1] [0] 

N-i; 

// 

X 

surfquadvertices 

[j+1]  [1] 

=: 

N; 

// 

y 

surfquadvertices 

[j  +  l]  [2] 

= 

0; 

// 

z 

surfquadvertices 

[j+2] [0] 

= 

N-i-1; 

// 

X 

surfquadvertices 

[j+2] (1] 

= 

N; 

// 

y 

surfquadvertices 

[j+21  [2] 

= 

0; 

// 

2 

surfquadvertices 

[j+3] [0] 

= 

0; 

// 

X 

surfquadvertices 

[j+3] [1] 

= 

i  +  l ; 

// 

y 

surfquadvertices 

[j+3]  [2] 

0; 

// 

z 

j  =  j  +  4; 

} 

if  (TRACE) 

cout  «  "surfquadvertices  block  2  complete,  j  range  =  ["  «  old_j«  " 
«  j-1  «  " ] "  «  endl ; 

for  (j  =  (1*N)*4;  j  <  (2*N)*4;  j++)  //  duplicate,  transpose  to  get  second 

half 
{ 

surfquadvertices  [j]  [0]  =  surfquadvertices  [j  -  (1*N) *4]  [0 ] ; //x<-x 
surfquadvertices  [j]  [1]  =  N-surfquadvertices [ j- (1*N) *4] [1] ;  //y<-(10”y) 
surfquadvertices  [j] [2]  =  -0.05;  //  z 

} 


//  quadrilaterals  now  have  opposite  normal,  switch  coordinates 

for  (i  =  40;  i  <  80;  i  +=  4)  If  (box  pattern  red) 

{ 

swap_float (surfquadvertices [i] [0] , surfquadvertices [i+3 ] [0]); 
//points  1,4  X 

swap_float (surfquadvertices [i] [1] , surfquadvertices [i+3 ] [1]); 

//points  1 , 4  y 
//  2  values  identical 

swap_float  (surfquadvertices  [i  +  l]  [0] ,  surfquadvertices  [i+2] [0] ) ; 
//  points  2,3  X 

swap_float  (surfquadvertices  [i+l][l],  surfquadvertices  [i+2][ll); 
//  points  2,3  y 


if  (TRACE) 

cout  «  "surface  quads  second  half  duplicate  &  transpose  complete,  " 

«  "j  range  =  ["  «  (1*N)*4  «  ",  "  «  (2*N)*4  -  1  «  "]"  «  endl; 

//  outline  square  last,  must  start  adjacent  to  last  point  to  avoid  artifact 
j  =  (2*N)*4; 

{ 


surfquadvertices 

[j 

] 

[0] 

=  N; 

// 

X 

surfquadvertices 

[j 

] 

[1] 

=  0; 

// 

y 

surfquadvertices 

[j 

] 

[2] 

=  0; 

if 

z 

surfquadvertices 

[j+1] 

[0] 

=  N; 

If 

X 

surfquadvertices 

[j+ 

1] 

[1] 

=  N; 

// 

y 

surfquadvertices 

[j+1] 

[2] 

=  0; 

fi 

z 
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surfquadvertices 

[j+2] 

[0] 

=  0; 

// 

X 

surfquadvertices 

[j+2] 

[1] 

=  N; 

// 

y 

surfquadvertices 

[j+2] 

[2] 

=  0; 

// 

z 

surfquadvertices 

[j+3] 

[0] 

=  0; 

// 

X 

surfquadvertices 

[j+3] 

[1] 

=  0; 

// 

y 

surfquadvertices 

(j+3] 

[2] 

=  0; 

// 

2 

j  =  j  +  4; 

} 

if  (TRACE) 

cout  «  "surfquadvertices  outline  square  block  complete,  j  range  =  [" 
«  (2*N)*4  «  ",  "  «  j-1  «  "]"  «  endl; 

//  use  consistent  definitions  with  terrain  box  «< 
const  int  BOXSCALE  =  40.0; 
const  int  OFFSET  =  200.0; 

for  (j  =  0;  j  <  (2*N+1)*4;  j++)  //  scale  and  translate  all  quads 

{ 

surf quadvert ices [j ] [0]  =  surfquadvertices  [ j ] [0 ] *B0XSCALE  -  OFFSET;  // 
surfquadvertices [ j ] [1]  =  surfquadvertices  [ j ] [1] *BOXSCALE  -  OFFSET;  // 

} 

if  (TRACE) 

cout  «  "surface  quads  rescale  and  transformation  complete."  «  endl; 

//  Define  coordinates  for  quad  vertices  &  SoFaceSet 
SoCoordinateS  *surf coord  =  new  SoCoordinateS; 
surf coord- >point . setValues  (0,  (2*N+1)*4,  surfquadvertices); 
cout  «  "surfcoord->point .setValues (0,  "  «  {2*N+1)*4 
«  ",  surfquadvertices)  complete."  «  endl; 

SoFaceSet  *surfquadset  =  new  SoFaceSet; 

surfquadset“>numVertices . setValues  (0,  21,  numbersurfvertices) ; 
cout  «  "surfquadset->numVertices . setValues  complete."  «  endl ; 
SoComplexity  *  surfaceComplexi ty  =  new  SoComplexity; 
surfaceComplexity->value  =  1.0;  . 

//  bring  together  all  the  surface  components 
SoSeparator  *surfsec‘tion  =  new  SoSeparator; 
surf section- >addChild(  surfaceComplexi ty  ) ; 
if  (MINEFIELD) 

{ 

surfsection->addChild(  MineFieldTransform  ); 
surfsection->addChild(  MineFieldScale  ); 

} 

surf sect ion->addChild(  wires  ); 

surf section->addChild (  seasurface  ); 

surf section->addChild(  surfcoord  ); 

surf section->addChild(  surfquadset  ) ; 

root->addChild (  surfsection  ); 

cout  «  "surface  wire  grid  complete."  «  endl; 

//  End  Ocean  surface  wiredraw  rectangles  using  FaceSet  - 

if  (PRINTDIALOG  ==  TRUE) 

{ 

//  Print  dialog  widget:  Inventor  training  manual  p.  9-9 
SoXtPrintDialog  *printDialog  =  new  SoXtPrintDialog; 
printDialog->setSceneGraph  (root) ; 
printDialog->show  (); 

//  printDialog->setWYSIWYG  (  TRUE  ); 

//  printDialog->setComponents  (  SoOffscreenRenderer : :RGB  ); 

) 

//  Uncomment  which  viewer  you  want  to  use: 

SoXtExaminerViewer  *  viewer  =  new  SoXtExaminerViewer; 

//  SoXtFlyViewer  *  viewer  =  new  SoXtFlyViewer; 

//  SoXtPlaneViewer  *  viewer  =  new  SoXtPlaneViewer; 

//  SoXtWalkViewer  *  viewer  =  new  SoXtWalkViewer; 

SbColor  background_bluecolor  (  0.1,  0.3,  0.5  ); 
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X  >1 


SbColor  background_blackcolor  (0.0, 0,0, 0.0); 

SbColor  background^greycolor  (  0.1,  0.1,  0.1  ) ; 

SbColor  background_whitecolor  (  1.0,  1.0,  1.0  ); 

if  (MINEFIELD)  viewer->setBackgroundColor (  background_greycolor  ); 
else  viewer~>setBackgroundColor (  background_bluecolor  ); 

/*  colored! tor  not  found? 1  superceded  by  SoXtMaterialEditor? 
if  (BACKGROUNDCOLORDIALOG  ==  TRUE) 

{ 

//  Build  the  color  editor  in  its  own  window 

SoXtColorEditor  *color_editor  =  new  SoXtColorEdi tor; 
color_editor~>build( ) ; 

color_editor“>setTitle (  "AUV  viewer  background  color"  ); 

//  Add  a  callback  for  when  the  color  changes 

color_editor->addColorChangedCallback (  colorEditorCB,  //  the  callback 

viewer  ) ;  //  user  data  to  be  passed 
color_editor->setColor  (  backbluecolor  ) ; 
color_editor->show{) ;  //  Display  the  color  editor 

} 

*/ 

char  window_title  [60]; 

strcpy  (window_title,  "NFS  AUV  Virtual  World  ("); 

strcat  (window__title,  "multicast  address  "); 

strcat  (window_title,  group) ; 

strcat  (window_title,  ",  "); 

strcat  (window_title,  "port  ") ; 

strcat  (window_title,  port); 

strcat  (window_title,  ")"); 

viewer->setSceneGraph (  root  ) ; 
viewer->setTitle (window_title) ; 
viewer-'>setDecoration  (0)  ; 
viewer->show( ) ; 

//  XtRealizeWidget  (  ViewerWindowWidget  );  //  mini  window  junk 

SoXt : imainLoop ( ) ;  //  loop  forever,  sending  events  to  the  scene  graph 

} 

///////////////////////////////////////////////////////////////////////////// 

//  Read  Circles  reads  from  a  file  containing  circle  descriptions  and  adds 
those 

//  circles  to  the  scene  graph 
void  readCircles  () 

{ 

double  circleX  =  0.0; 

double  circleY  =0.0; 

double  circleZ  =  0.0; 

double  circleRadius  =  0.0; 

char  circleFlag  [50]  ; 

SoSeparator  *  allCirclesSeparator; 

SoSeparator  *  circleSeparator; 

SoTranslation  *  circleTranslate; 

SoRotationXYZ  *  circleRotation  =  new  SoRotationXYZ; 

SoSeparator  *  minePairSeparator; 

SoSeparator  *  mineSeparator; 

SoTranslation  *  mineTranslate; 

circleRotation->angle  =  M_PI/2; 
circleRotation- >axis  =  SoRotationXYZ : :X; 

SoMaterial  *  circleMaterial  =  new  SoMaterial; 
circleMaterial->ambientColor . setValue  (1.0, 0.0, 0.0) ; 
circleMaterial->dif fuseColor . setValue  (1 . 0, 0 . 0, 0 . 0) ; 

//  circleMaterial->emissiveColor.setValue{1.0,0.0,0.0) ;  //  kills  shading 

//  transparency  >  0.5  shifts  to  SCREENDOOR  and  looks  horrible  :( 
circleMaterial->transparency. setValue (0.5); 
circleMaterial->shininess . setValue (1.0) ; 

SoCylinder  *  circleCylinder; 
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SoPickStyle  *  pickablestylenode; 
pickablestylenode  =  new  SoPickStyle; 

pickablestyl€node->style. setValue  {  SoPickStyle SHAPE  ); 

SoComplexity  *  cylinderComplexity  =  new  SoComplexity; 
cylinderCoiTiplexity~>value  =  0.3; 

allCirclesSeparator  =  new  SoSeparator; 
scenery->addChild  (  allCirclesSeparator  ); 
allCirclesSeparator->addChild  (  cylinderComplexity  ); 
allCirclesSeparator->addChild  (  circleMaterial  ); 
allCirclesSeparator->addChild  {  pickablestylenode  ); 

char  circleFileLine  [80]; 

for  (int  index  =  0;  index  <  80;  index++)  circleFileLine  [index]  =  '\0'; 

while  {circleFile.getline  {circleFileLine,  80)) 

{ 

if  (TRUE)  cout  «  "["  «  circleFileLine  « 
int  paramcount  =  sscanf  (circleFileLine,  "%s  %lf  %lf  %lf  %lf", 

SccircleFlag,  &circleX,  &circleY,  SccircleZ,  &c.ircleRadius)  ; 
for  (index  =  0;  index  <  80;  index++)  circleFileLine  [index]  =  '\0'; 

if  (TRUE) 

{ 

cout  «  endl; 

cout  «  "[parsed  from  circle  file:  " 

«  circleFlag  «  "  "  «  circleX  «  "  "  «  circleY  «  "  " 

«  circleZ  «  "  "  «  circleRadius  «  "]"  «  endl; 

} 

for  (int  index  =  0;  index  <=  strlen  (circleFlag);  index++)  //uppercase 
circleFlag  [index]  =  toupper  (circleFlag  [index]); 

if  (((strcmp  (circleFlag, "CIRCLE" )  ==  0)  II 

(strcmp  (circleFlag, "CYLINDER")  ==  0)  )  && 

(paramcount  ==  5)  &&  (circleRadius  >  0.0)) 

{ 

circleSeparator  =  new  SoSeparator; 
circleTranslate  =  new  SoTranslation; 
circleCylinder  =  new  SoCylinder; 

circleCylinder->parts  =  So(^linder; : SIDES;  //  let's  see  through! 
allCirclesSeparator '->addChild  {  circleSeparator  )  ; 
circleTranslate-->translation.setValue  (circleX,  -circleY, -circleZ)  ; 
circleSeparator->addChild  (  circleTranslate  ) ; 
circleSeparator->addChild  (  circleRotation  ); 
circleCylinder->radius . setValue (circleRadius) ; 
circleCylinder->height.setValue (2 .0  *  circleZ); 
circleSeparator->addChild  (  circleCylinder  ); 

} 

else  if  ((strcmp  (circleFlag, "MINE")  ==  0)  && 

(paramcount  >=  4) ) 

{ 

minePairSeparator  =  new  SoSeparator; 
allCirclesSeparator->addChild  (  minePairSeparator  ) ; 
mineTranslate  =  new  SoTranslation; 

mineTranslate->translation. setValue (circleX, -circleY, -circleZ) ; 
minePairSeparator->addChild  {  mineTranslate  ) ; 
mineSeparator  =  readFile  ( "mine_spiked. iv" ) ; 
minePairSeparator->addChild  (  mineSeparator  ); 

} 

else  if  (TRUE)  cout  «  "[failed  parse  test]"  «  endl; 

} 

}  / /  end 

///////////////////////////////////////////////////////////////////////////// 

//  Sonar  Range  Function.  Temporary  Addition  for  Testing,  moved  to  dynamics 
double  auv_ST725_range  (  double  sonarBearing,  double  power,  double  maxSonar- 
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Range, 

SbViewport Region  *  pickViewPort  ) 

power  =  0.0;  //  unused 

double  sinPhi  =  sin  (AUV_phi), 
cosPhi  =  cos  (AUV_phi), 
sinTheta  =  sin  (AUV_theta) , 
cosTheta  =  cos  (AUV_theta), 
sinPsi  =  sin  (AUV_psi), 
cosPsi  =  cos  (AUV_psi); 

//  SbViewportRegion  pickViewPort; 

//  Positions  of  the  start  and  end  of  the  ray  in  AUV  Coordinates 
float  beamOffsetX  =  AUV_ST72 5_x_of f set  /  12.0, 
beamOffsetY  =  AUV_ST72  5^_of f set  /  12.0, 
beamOffsetZ  =  AUV_ST725__z_of f set  /  12.0, 
beamAxisX  =  cos  (radians  (sonarBearing) ) ,  //  3D 
beamAxisY  =  sin  (radians  (sonarBearing)); 

//  beamAxisZ  =0;  //  unchanging 


//  Positions  of  the  start  and  end  of  the  ray  in  World  Coordinates 


float  worldBeamOriginX 


wo  r 1 dBeamOr i gi nY 


wo  r  1  dBeaxnOrigi  nZ 


beamOffsetX  * 
beamOffsetY  * 

beamOffsetZ  * 

AUV_x, 

beamOffsetX  * 
beamOffsetY  * 

beamOffsetZ  * 

AUV_y, 

beamOffsetX  * 
beamOffsetY  * 
beamOffsetZ  * 
AUV_z ; 


(cosPsi  *  cosTheta)  + 

(cosPsi  *  sinTheta  *  sinPhi  - 
sinPsi  *  cosPhi)  + 

(cosPsi  *  sinTheta  *  cosPhi  + 
sinPsi  *  sinPhi)  + 

(sinPsi  *  cosTheta)  + 

(sinPsi  *  sinTheta  *  sinPhi  + 
cosPsi  *  cosPhi)  + 

(sinPsi  *  sinTheta  *  cosPhi  - 
cosPsi  *  sinPhi)  + 

(-sinTheta)  + 

(cosTheta  *  sinPhi)  + 
(cosTheta  *  cosPhi)  + 


float  worldBeamAxisX  =  beamAxisX  * 

beamAxisY  * 

worldBeamAxisY  =  beamAxisX  * 
beamAxisY  * 

worldBeamAxisZ  =  beamAxisX  * 
beamAxisY  * 


(cosPsi  *  cosTheta)  + 

(cosPsi  *  sinTheta  *  sinPhi  *- 
sinPsi  *  cosPhi ) , 

(sinPsi  *  cosTheta)  + 

(sinPsi  *  sinTheta  *  sinPhi  + 
cosPsi  *  cosPhi), 

(-sinTheta)  + 

(cosTheta  *  sinPhi); 


double  range  =  0.0; 


SoRayPickAction  pingPickAction  {*pickViewPort) ; 

pingPickAction. setRay  (SbVec3f  (worldBeamOriginX,  -worldBeamOriginY,  -world- 
BeamOriginZ) , 

SbVec3f  (worldBeamAxisX,  -worldBeamAxisY,  -worldBeamAx¬ 
isZ)  , 

0.0, maxSonarRange ) ; 


pingPickAction , apply  (  scenery  ); 


const  SoPickedPoint  *pingPickedPoint  =  pingPickAction. getPickedPoint  (); 
SbVec3f  pingPoint; 


cout  «  "[ST725  Position  "  «  worldBeamOriginX  «  "  "  «  worldBeamOriginY 
«  "  "  «  worldBeamOriginZ  «  «  endl; 

cout  «  "[ST725  Beam  Axis  "  «  worldBeamAxisX  «  "  "  «  worldBeamAxisY 
«  "  "  «  worldBeamAxisZ  «  «  endl; 
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if  (pingPickedPoint  !=  NULL) 

{ 

pingPoint  =  pingPickedPoint->getPoint  (); 

cout  «  " [ST725  ping  picked  point  "  «  pingPoint [0]  «  "  " 

«  -pingPoint [1]  «  "  "  «  -pingPoint [2]  «  «  endl; 

} 

else 

{ 

cout  «  " [ST725  ping  function  did  not  pick  any  points]"  «  endl; 
pingPoint  =  SbVec3f  (worldBeainOriginX, -worldBeamOriginY, -worldBeamOriginZ)  ; 
} 


range  =  sqrt 


(  ( (worldBeamOriginX  - 
(worldBeamOriginX  - 
( (worldBeamOriginY  - 
(worldBeamOriginY  - 
( (worldBeamOriginZ  - 
(worldBeamOriginZ  - 


pingPoint [0] )  * 
pingPoint [0] ) )  +, 
-pingPoint [1] )  * 
-pingPoint [1] ) )  + 
-pingPoint [2] )  * 
-pingPoint [2] ) ) ) ; 


cout  «  "[ST725  Sonar  Range:  "  «  range  «  "]"  «  endl; 
cout  «  "[End  pingST725]"  «  endl; 


return  (range); 

}  //  end  auv_ST7 2 Strange 


//  end  of  viewer. C 

///////////////////////////////////////////////////////////////////////////// 


///////////////////////////////////////////////////////////////////////////// 

/* 

Program: World  database  geometric  sonar  model:  ivSonar.C 

Description; Geometric  sonar  model  for  scene  graph. 

Author: Duane  Davis,  Don  Brutzman 

Revised: 20  June  96 

System: I rix  5.3 

Compiler : ANSI  C++ 

Compilation: irix>  make  ivSonar.o 

irix>  CC  ivSonar.C  -Im  -c  -g  +w 

-c  ==  Produce  binaries  only,  suppressing  the  link  phase. 

+w  ==  Warn  about  all  questionable  constructs. 

Description: Generic  sonar  model  for  Pheonix  AUVST725  and  STIOOO  sonars 
using  Open  Inventor  ray  pick  action  and  viewer  scene  graph 

*/ 

///////////////////////////////////////////////////////////////////////////// 

#include  "AUVglobals . H" 

#include  <iostream.h> 

#include  <f stream. h> 

#include  <iomanip.h>  //  must  follow  iostream.h 
#include  <string.h> 

#include  <math.h> 

#include  <time.h> 

#include  <getopt.h> 
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#include  <stdlib.h> 

#include  <netdb.h> 

#include  <netinet/in.h> 

#include  <sys/types . h> 

#include  <ctype.h> 

///////////////////////////////////////////////////////////////////////////// 

#ifdef  INVENTOR 

#include  <X11/Intrinsic . h> 

#include  <Xll/keysym.h> 

#include  <Xin/Xin.h> 

#include  <Xm/CascadeBG.h> 

#include  <Xin/Fonn.h> 

#include  <Xin/RowColuinn.h> 

#include  <Xin/PushB.h> 

# include  <Xm/PushBG.h> 

#include  <Xm/Separator .h> 

#include  <Xin/SeparatoG,h> 

#include  <Xin/ToggleB . h> 

# include  <Xm/ToggleBG.h> 

#include  <Inventor/SbBasic.h> 

#include  <Inventor/SbViewportRegion.h> 

#include  <Inventor/So .h> 

#include  <Inventor/SoDB.h> 
tinclude  <Inventor/SoInput .h> 
tinclude  <Inventor/SoPick:edPoint .  h> 

#include  <Inventor/Xt/SoXt .h> 

#include  <Inventor/Xt/SoXtPrintDialog.h>  //  see  SoOf f screenRender 
#include  <Inventor/Xt/SoXtRenderArea . h> 
tinclude  <Inventor/Xt/SoXtMaterialEditor ♦h> 
tinclude  <Inventor/Xt /viewers /SoXtExaininerViewer.h> 

tinclude  <Inventor/actions/SoAction .h> 
tinclude  <Inventor/actions/SoCallbackAction .h> 
tinclude  <Inventor/actions/SoGLRenderAction .h> 
tinclude  <Inventor/accions/SoWriteAction . h> 
tinclude  <InvenCor/actions/SoPickAction . h> 
tinclude  <Inventor/actions/SoRayPickAction.h> 

tinclude  <Inventor/engines/SoCalculator .h> 
tinclude  <Inventor/engines/SoElapsedTime.h> 
tinclude  <Inventor/ engines /SoTimeCounter . h> 

tinclude  <Inventor/events/SoEvent .h> 
tinclude  <Inventor/events/SoKeyboardEvent .h> 

tinclude  <Inventor/nodes/SoCoinplexity  .h> 
tinclude  <Inventor/nodes/SoCone . h> 

tinclude  <Inventor/nodes/SoCoordinate3 .h> 
tinclude  <Inventor/nodes/SoCube .h> 
tinclude  <Inventor/nodes/SoCylinder .h> 
tinclude  <Inventor/nodes/SoCone .h> 
tinclude  <Inventor/nodes/SoDirectionalLight .h> 
tinclude  <Inventor/nodes/SoDrawStyle . h> 
tinclude  <Inventor/nodes/SoEventCallback.h> 
tinclude  <Inventor/nodes/SoFaceSet . h> 
tinclude  <Inventor/nodes/SoGroup . h> 
tinclude  <Inventor/nodes/SoMaterial .h> 
tinclude  <Inventor/nodes/SoNurbsSurface .h> 
tinclude  <Inventor/nodes/SoPerspectiveCamera,h> 
tinclude  <Inventor/nodes/SoPickStyle,h> 
tinclude  <Inventor/nodes/SoRotationXYZ .h> 
tinclude  <Inventor/nodes/SoScale. h> 
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#include  <Inventor/nodes/SoSelection.h> 

#include  <Inventor/nodes/SoSeparator.h> 

#include  <Inven tor/nodes /SoSphere.h> 

#include  <Inventor/nodes/SoSwitch.h> 

#include  <Inventor/nodes/SoTransfonn.h> 

#include  <Inventor/nodes/SoTransfonnSGparator .h> 

#include  <Inventor/nodes/SoTranslation,h> 

#include  <Inventor/nodes/SoTriangleStripSet .h>  //  order  matters  here 
#include  <Inventor/nodes/SoUnits .h> 

/////////////////////////////////////////////////////////////////////////////. 
SoSeparator  *  scenery  =  new  SoSeparator;  //  objects  which  can  be  pinged 
SoSeparator  *  readFile (const  char  * filename ) ;  //  Inventor  Mentor  p.  284 
SbViewportRegion  pickViewPort ;  //  required  for  pick  functions  to  work 

void  readCircles  (); 


///// III ! f / n 1 1 1 1 ! I N  f 1 1 ! n I U  n I n I ! m 1 1 1 1 ! I ! 1 1 ! 1 1 f ! I u ! i I i u ! f 1 1 1 1 j  j i I { f  f I 

double  radians  (double) ; 

extern  int  READCIRCLEFILE; 

extern  int  LOADDEFAULTWORLD; 

extern  ifstream  circleFile; 

///////////////////////////////////////////////////////////////////////////// 

void 

setupinventor  (char  *  argument) 

{ 

//  Initialize  Inventor  and  Xt  -  these  steps  MUST  be  first  calls 
//  in  main,  without  exception,  or  a  mystery  crash  results. 

Widget  ViewerWindowWidget  =  SoXt :: ini t (argument) ; 
if  (  ViewerWindowWidget  ==  NULL  ) 

{ 

cout  «  "viewer:  ViewerWindowWidget  ==  NULL  on  startup,  exiting." 

«  endl; 
exit(  1  ).; 

} 

return; 

}  //  end  setupinventor 


///////////////////////////////////////////////////////////////////////////// 

void 

loadSceneGraph  () 

{ 

static  int  sceneCreated  =  0; 

if  (sceneCreated)  return;  //  don't  load  scene  more  than  once 

if  (TRACE)  cout  «  "[Begin  loadSceneGraph]"  «  endl; 

sceneCreated  =1;  //  Don't  allow  scene  to  be  created  again 

scenery “>rGf () ; 

if  (LOADDEFAULTWORLD) 

{ 

//  All  objects  in  the  scenery  graph  are  pickable 
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SoPickStyle  *  pickablestylenode  =  new  SoPickStyle; 
pickablestylenode->style, setValue  (  SoPickStyle :: SHAPE  ); 
scenery->addChild (pickablestylenode) ; 

//  Platform 

SoSeparator  *  sepPlatform  =  new  SoSeparator; 

SoTransform  *  xfPlatform  =  new  SoTransfonn; 

SoSeparator  *  Platform  =  readFile  ("../viewer/Platform.iv"); 
xfPlatforin->translation.setValue{-80.0,  50.0,  -50.0); 
sepPlatform->addChild  (  xfPlatform  ); 
sepPlatform->addChild(  Platform  ); 
scenery->addChild  (  sepPlatform  ) ; 

//  Testtank 

SoSeparator  *  sepTesttank  =  new  SoSeparator; 

SoTransform  *  xfTesttank  =  new  SoTransform; 

SoSeparator  *  Testtank  =  readFile  (" . . /viewer/testtank.iv" ) ; 
if  (TESTTANKSURFACE) 

xfTesttank->translation.setValue(  0.0,  0.0,  -6.0); 

else 

xfTesttank->translation.setValue{‘  0.0,  0.0,  -50.0); 
sepTesttank->addChild  {  xfTesttank  ); 
sepTesttank->addChild(  Testtank  ); 
scenery->addChild  (  sepTesttank  ) ; 

//  Torpedo  Tube 

SoSeparator  *  sepTorpedoTube  =  new  SoSeparator; 

SoTransform  *  xfTorpedoTube  =  new  SoTransform; 
xfTorpedoTube->translation.setValue(  30.0,  20.0,  -48.00  ),* 

SoCylinder  *  TorpedoTube  =  new  SoCylinder; 

TorpedoTube->radius  =  21.0  /  12.0;  //  21"  diameter 

TorpedoTube->height  =  10.0;  //  lO'  length 

TorpedoTube->parts  =  SoCylinder: : SIDES;  //  let's  drive  through* 
sepTorpedoTube->addChild  (  xfTorpedoTube  ); 
sepTorpedoTube->addChild(  TorpedoTube  ); 
scenery->addChild  (  sepTorpedoTube  ); 

SoCube  *  backgroundCube  =  new  SoCube; 
backgroundCube->width  =  400.0; 
backgroundCube->height  =  400.0; 
backgroundCube->d€pth  =  0.1; 

SoTransform  *  xf backgroundCube  =  new  SoTransform; 
xfbackgroundCube->translation.setValue(0.0,  0.0,  -50.0) ; 

SoSeparator  *  sepbackgroundCube  =  new  SoSeparator; 
sepbackgroundCube->addChild{  xf backgroundCube  ); 

sepbackgroundCube->addChild(  backgroundCube  );  //  remove  cube  here 

scenery ->addChi Id  (  sepbackgroundCube  ); 


//  WriteAction  writes  scene  graph  to  file 

FILE  *  scenery_iv_fp  =  fopen  {"dynamics_scenery. iv",  "w"); 

SoWriteAction  writeaction; 

writeaction.getOutput ()->  setFilePointer  (scenery_iv_fp) ; 
writeaction. apply  (scenery) ; 
f close  (scenery_iv_fp)  ; 

cout  «  "  writeaction. apply  (scenery)  =>  dynamics_scenery . iv  complete." 
«  endl ; 


if  (TRACE)  cout  «  "[End  loadSceneGraph] "  «  endl; 

}  If  end  loadSceneGraph 

/ //////////////////////////////////////////////////////////////////////////// 
double  pingST725  {  double  maxSonarRange,  double  powerSetCing  ) 

//  int  TRACE  =  1; 
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if  (TRACE)  cout  «  "[Begin  pingST725]"  «  endl; 

int  sonarBin[64]  =  {0}; 
int  bin; 

int  bestBin  =  0; 

double  binSize  =  maxSonarRange  /  64,0; 

SoPickedPoint  *pingPickedPoint ; 

SbVecSf  pingPoint; 

double  range  =  0,0; 

double  sinPhi  =  sin  (AUV_phi), 
cosPhi  =  cos  (AUV_jphi), 
sinTheta  =  sin  (AUV_theta), 
cosTheta  =  cos  (AUV_theta), 
sinPsi  =  sin  (AUV^psi), 
cosPsi  =  cos  (AUV_psi); 

powersetting  =  10.0;  //  currently  unused 

//  Positions  of  the  start  and  end  of  the  ray  in  AUV  Coordinates 
//  Beam  Axis  is  for  center  of  24  degree  vertical  arc 
//  Beam  Axis  must  be  recomputed  for  top  and  bottom  of  beam 
float  beamOffsetX  =  AUV_ST725_x_of f set  /  12.0, 
beamOffsetY  =  AUV_ST72  5_y_of fset  /  12,0, 
beamOffsetZ  =  AUV_ST725_z_of f set  /  12.0, 

beamAxisX  =  cos  (radians  (AUV_ST725_bearing)  ) , 

beamAxisY  =:  sin  (radians  (AUV_ST725_bearing) )  , 

beainAxisZ  =  0; 


//  Positions  of  the  start  and  end  of  the  ray  in  World  Coordinates 


float  worldBeamOriginX 


worl dBeamOriginY 


worldBeamOriginZ 


beamOffsetX  * 
beamOffsetY  * 

beamOffsetZ  * 

AUV_x, 

beamOffsetX  * 
beamOffsetY  * 

beamOffsetZ  * 

AUV_/, 

beamOffsetX  * 
beamOffsetY  * 
beamOffsetZ  * 
AUV_z  ; 


(cosPsi  *  cosTheta)  + 

(cosPsi  *  sinTheta  *  sinPhi  - 
sinPsi  *  cosPhi)  + 

(cosPsi  *  sinTheta  *  cosPhi  + 
sinPsi  *  sinPhi)  + 

(sinPsi  *  cosTheta)  + 

(sinPsi  *  sinTheta  *  sinPhi  + 
cosPsi  *  cosPhi)  + 

(sinPsi  *  sinTheta  *  cosPhi  - 
cosPsi  *  sinPhi)  + 

(-sinTheta)  + 

(cosTheta  *  sinPhi)  + 
(cosTheta  *  cosPhi)  + 


float  worldBeamAxisX, 
worldBeamAxisY, 
worldBeamAxisZ; 


SoRayPickAction  pingPickAction  (pickViewPort) / 

for  (double  rayElevation  =  -12.0;  rayElevation  <=  12.0;  rayElevation  +=  2.0) 
{ 

beamAxisZ  =  tan  (  rayElevation  *  M_PI  /  180.0); 


worldBeamAxisX  =  beamAxisX  * 
beamAxisY  * 

beamAxisZ  * 

worldBeamAxisY  =  beamAxisX  * 


(cosPsi  *  cosTheta)  + 

(cosPsi  *  sinTheta  *  sinPhi  - 
sinPsi  *  cosPhi)  + 

(cosPsi  *  sinTheta  *  cosPhi  + 
sinPsi  *  sinPhi), 

(sinPsi  *  cosTheta)  + 
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beamAxisY  *  (sinPsi  *  sinTheta  *  sinPhi  + 
cosPsi  *  cosPhi)  + 

beamAxisZ  *  (sinPsi  *  sinTheta  *  cosPhi  - 
cosPsi  *  sinPhi) , 

worldBeamAxisZ  =  beamAxisX  *  {-sinTheta)  + 

beamAxisY  *  (cosTheta  *  sinPhi)  + 
beamAxisZ  *  (cosTheta  *  cosPhi) ; 

//  Perform  pick 

pingPickAction . setRay (SbVec3f (worldBeamOriginX/ 

-worldBeamOriginY, 
-worldBeamOriginZ) , 

SbVec3f (worldBeamAxisX, 
-worldBeamAxisY, 
-worldBeamAxisZ)  / 
O.O^maxSonarRange) ; 

if  (TRACE) 

{ 

cout  «  "[ST725  Position  "  «  worldBeamOriginX  «  "  " 

«  WorldBeamOriginY 

«  "  "  «  WorldBeamOriginZ  «  «  endl; 

cout  «  "  [ST725  Beam  Axis  "  «  worldBeamAxisX  «  "  " 

«  worldBeamAxisY 

«  "  "  «  worldBeamAxisZ  «  «  endl; 

} 


pingPickAction. apply  {  scenery  ); 
pingPickedPoint  =  pingPickAction .getPickedPoint  (); 

if  (pingPickedPoint  i=  NULL) 

{ 

pingPoint  =  pingPickedPoint->getPoint  (); 
if  (TRACE) 

cout  «  "[ST725  "  «  rayElevation  «  "  Degree  Ray  Picked  Point" 
«  pihgPoint[0]  «  "  " 

«  -pingPoint [1]  «  "  "  «  -pingPoint [2 ]  «  "]"  «  endl; 
range  =  sqrt  (({worldBeamOriginX  -  pingPoint [0] }  * 

(worldBeamOriginX  -  pingPoint [0] ) )  + 
((worldBeamOriginY  +  pingPoint [1] )  * 

(worldBeamOriginY  +  pingPoint [1] ) )  + 
((worldBeamOriginZ  +  pingPoint [2 ] )  * 

^  (worldBeamOriginZ  +  pingPoint [2] ) ) ) ; 

else 

{ 

if  (TRACE) 

cout  «  "[ST725  "  «  rayElevation 

«  "  degree  ray  did  not  Pick  any  Points]"  «  endl; 
range  =  10000.0;  //  a  big  number  that  will  excede  max  sonar  range 


if  (TRACE)  cout  «  "[ST725  "  «  rayElevation  «  "  degree  range:  " 
«  range  «  "]"  «  endl; 

bin  =  (int)  (range  /  binSize); 

if  (bin  <  64) 

{ 

if  (TRACE)  cout  «  "[Incrementing  bin  number:  "  «  bin  «  "]" 
«  endl; 

++sonarBin [bin] ; 

} 


//  Determine  range  to  return 
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for  (int  i  =  0/  i  <  64;  i++) 

{ 

if  (sonarBin[i]  >  sonarBin [bestBin] ) 
bestBin  =  i; 

} 

if  (sonarBin [bestBin]  >  1) 

{ 

//  range^error:  +|-  (0..1]  *  inax_percentage_error  *  sonar_return_value 

double  sonar_return_value  =  (binSize  *  bestBin  +  binSize  /  2.0); 
double  range_error  =  sonar_return_value  * 

{niax_pct_error_ST72  5  /  100.0)  * 

(2.0  *  (drand48  ( )  ->  0.5)); 

if  (TRACE)  cout  <<  "ST_725  range  =  "  «  sonar_return_value  «  ", 
if  (TRACE)  cout  «  "range  error  =  "  «  range_error  «  "• 

sonar_return_value  +=  range_error; 

if  (TRACE)  cout  «  "error  return  = «  sonar_r€turn_value  «  endl; 
return  (sonar_return__value)  ; 

} 

else  return  (0.0); 

}  //  end  pingST725 


///////////////////////////////////////////////////////////////////////////// 
double  pingSTlOOO  (  double  maxSonarRange,  double  powersetting  ) 

//  int  TRACE  =  1; 

if  (TRACE) 

cout  «  "[Begin  pingSTlOOO]"  «  endl; 

double  sinPhi  =  sin  (AUV_phi), 
cosPhi  =  cos  (AUV_phi), 
sinTheta  =  sin  (AUV__theta) , 
cosTheta  =  cos  (AUV_theta), 
sinPsi  =  sin  {AUV_psi), 
cosPsi  =  cos  (AUV^psi); 

powersetting  =  10.0;  //  currently  unused 

//  SbViewportRegion  pickViewPort; 

//  Positions  of  the  start  and  end  of  the  ray  in  AUV  Coordinates 

float  beamOffsetX  =  AUy_ST1000_x_of f set  /  12.0, 

beamOffsetY  =  AUV_ST1000_y_of fset  /  12.0, 

beamOffsetZ  =  AUV_ST1000_z_of fset  /  12.0, 

beamAxisX  =  cos  (radians  (AUV_ST1000_bearing) ) , 

beamAxisY  =  sin  (radians  (AUV_ST1000_bearing) ) ; 

//  beamAxisZ  =0;  //  unchanged 

//  Positions  of  the  start  and  end  of  the  ray  in  World  Coordinates 


float  worldBeamOriginX  =  beainOffsetX  * 

beamOffsetY  * 

beamOffsetZ  * 

AUV_x, 

worldBeamOriginY  =  beamOffsetX  * 
beamOffsetY  * 

beamOffsetZ  * 


(cosPsi  *  cosTheta)  + 

(cosPsi  *  sinTheta  *  sinPhi  - 
sinPsi  *  cosPhi)  + 

(cosPsi  *  sinTheta*  cosPhi  + 
sinPsi  *  sinPhi)  + 

(sinPsi  *  cosTheta)  + 

(sinPsi  *  sinTheta  *  sinPhi  + 
cosPsi  *  cosPhi)  + 

(sinPsi  *  sinTheta  *  cosPhi  - 
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cosPsi 


sinPhi)  + 


AUV_y, 

worldBeamOriginZ  =  beamOffsetX  *  (-sinTheta)  + 

beamOffsety  *  (cosTheta  *  sinPhi)  + 
beamOffsetZ  *  (cosTheta  *  cosPhi)  + 

AUV_z  ; 

float  worldBeamAxisX  =  beamAxisX  *  (cosPsi  *  cosTheta)  + 

beamAxisY  *  (cosPsi  *  sinTheta  *  sinPhi  - 
sinPsi  *  cosPhi), 

worldBeainAxisY  =  beamAxisX  *  (sinPsi  *  cosTheta)  + 

beamAxisY  *  (sinPsi  *  sinTheta  *  sinPhi  + 
cosPsi  *  cosPhi) , 

worldBeamAxisZ  =  beamAxisX  *  (-sinTheta)  + 

beamAxisY  *  (cosTheta  *  sinPhi); 

double  range  =  0.0; 

SoRayPickAction  pingPickAction  (pickViewPort) ; 
pingPickAction . setRay (SbVec3  f (worldBeamOriginX, 

-worldBeamOriginY, 

-WorldBeamOriginZ) , 

SbVecSf (worldBeamAxisX, 

- wor 1 dBe amAx i s Y , 

-worldBeamAxisZ) , 

0.0, maxSonarRange ) ; 

pingPickAction . apply  (  scenery  ); 

const  SoPickedPoint  *pingPickedPoint  =  pingPickAction . getPickedPoint  () 
SbVec3f  ping Point; 

if  (TRACE) 

{ 

cout  «  "[STIOOO  Position  "  «  worldBeamOriginX  «  "  " 

«  worldBeamOriginY 

«  "  "  «  worldBeamOriginZ  «  «  endl; 

cout  «  "  [STIOOO  Beam  Axis  "  «  worldBeamAxisX  «  "  " 

«  worldBeamAxisY 

«  "  "  «  worldBeamAxisZ  «  «  endl; 


if  (pingPickedPoint  J=  NULL) 

{ 

pingPoint  =  pingPick€dPoint->getPoint  (); 
if  (TRACE) 

cout  «  ''[STIOOO  Ping  Picked  Point  "  «  pingPoint[0]  «  "  " 

^  «  -pingPoint  [1]  «  "  "  «  -pingPoint  [2  ]  «  "]"  «  endl; 

else 

{ 

if  (TRACE)  cout  «  "[STIOOO  Ping  Function  did  not  Pick  any  Points]" 
«  endl; 

pingPoint  =  SbVec3f  (worldBeamOriginX, 

-worldBeamOriginY, 

-worldBeamOriginZ) ; 


range  =  sqrt  {((worldBeamOriginX  -  pingPoint [0 ] )  * 

(WorldBeamOriginX  -  pingPoint [0 ]) )  + 

{(worldBeamOriginY  -  -pingPoint [1] )  * 

(WorldBeamOriginY  -  -pingPoint [ 1 ]) )  + 

((worldBeamOriginZ  -  -pingPoint [2 ] )  * 

(WorldBeamOriginZ  -  -pingPoint  [2] ))) ; 

//  range_error:  +|-  (0..1]  *  max__percentage_error  *  sonar_return_value 

double  sonar__return_value  =  range; 
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double  range_error  =  sonar_rGturn_value  * 

{inax_pct_Grror_ST1000  /  100.0)  * 

(2.0  *  {drand48  ()  -  0.5) ) ; 

if  (TRACE)  cout  «  "STIOOO  range  =  "  «  sonar_return_valuG  «  ", 
if  (TRACE)  cout  «  "range  error  =  "  «  range_error  «  ",  " ; 

sonar_return_value  +=  range_error; 

if  (TRACE)  cout  «  "error  return  =  "  «  sonar_return_value  «  endl ; 
if  (TRACE)  cout  «  "[End  pingSTlOOO)"  «  endl; 
return  (sonar_return_valuG) ; 

}  //  end  pingSTlOOO 

///////////////////////////////////////////////////////////////////////////// 

void 

addWorldPile  (const  char  *filenaine) 

{ 

SoSeparator  *  worldSeparator  =  readFile  (filename) ; 
if  { iworldSeparator) 

{ 

char  newfilename  [50]  ; 
strcpy  (newfilename,  ./viewer/"); 
strcat  (newfilename,  filename); 
worldSeparator  =  readFile  (newfilename) ; 

} 

if  (worldSeparator) 

{ 

scenery->addChild  (worldSeparator) ; 

cout  «  "[World  File  "  «  filename  «  "  opened]"  «  endl; 

} 

else 

{ 

cout  «  "Unable  to  locate  world-file  "  «  filename  «  cout; 
exit  (-1) ; 

} 


///////////////////////////////////////////////////////////////////////////// 

SoSeparator  *  readFile (const  char  *filename)  //  Inventor  Mentor  p.  284 
{ 

//  Open  the  input  file 

Soinput  n^Scenelnput; 

if  ( imySceneInput.openFile (filename) ) 

{ 

cout  «  "Cannot  open  file  "  «  filename  «  endl; 
return  NULL; 

) 

//  Read  the  whole  file  into  the  database 
SoSeparator  *  myGraph  =  SoDB :: readAll (&my Scene Input ) ; 
if  (myGraph  ==  NULL) 

{ 

cout  «  "Problem  reading  file  "  «  filename  «  endl; 
return  NULL; 

} 

myScenelnput .closeFile  ( )  ; 
return  myGraph; 

} 

///////////////////////////////////////////////////////////////////////////// 

//  Read  Circles  reads  from  a  file  containing  circle  descriptions  and  adds 
//  those  circles  to  the  scene  graph 
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void  readCircles  () 

{ 

double  circleX,  circleY,  circleZ,  circleRadius; 
char  circleFlag [50] ; 

SoSeparator  *  allCirclesSeparator; 

SoSeparator  *  circleSeparator; 

SoTranslation  *  circleTranslate; 

SoSeparator  *  ininePairSeparator; 

SoSeparator  *  mineSeparator; 

SoTranslation  *  mineTranslate; 

SoRotationXYZ  *  circleRotation  =  new  SoRotationXYZ; 

circleRotation->angle  =  M_PI/2; 
circleRotation->axis  =  SoRotationXYZ : :X; 

SoMaterial  *  circleMaterial  =  new  SoMaterial; 

//  circleMaterial->ambientColor .setValue  (1 . 0, 0 , 0 , 0 . 0 ) ; 

//  circleMaterial“>di f fuseColor . setValue  (1.0,0.0,0.0); 

//  circleMaterial->emissiveColor. setValue (1. 0,0. 0,0.0)  ; 
circleMaterial~>shininess . setValue (0 . 0) ; 

SoCylinder  *  circleCylinder; 

SoPickStyle  *  pickablestylenode; 
pickablestylenode  =  new  SoPickStyle; 

pickablestylenode->style.  setValue  (  SoPickStyle  :: SHAPE 
SoCoiuplexity  *  cylinderComplexity  =  new  SoComplexity; 
cylinderComplexity->value  =  0.4; 

allCirclesSeparator  =  new  SoSeparator; 
scenery->addChild  {  allCirclesSeparator  ) ; 
allCirclesSeparator->addChild  {  cylinderComplexity  ); 

//  allCirclesSeparator->addChild  (  circleMaterial  ); 
allCirclesSeparator->addChild  (  pickablestylenode  ) ; 

char  circleFileLine  [80]; 

for  (int  index  =  0;  index  <  80;  index++)  circleFileLine  [index]  =  '\0'; 
while  (circleFile .getline  (circleFileLine,  80)) 

{ 

if  (TRUE)  cout  «  "["  «  circleFileLine  «  "]"  «  endl; 
int  paramcount  =  sscanf  (circleFileLine,  "%s  %lf  %lf  %lf  %lf", 

SccircleFlag,  &circleX,  &circleY,  &circleZ,  &circleRadius)  ; 
for  (index  =  0;  index  <  80;  index++)  circleFileLine  [index]  =  '\0'; 

if  (TRUE) 

{ 

cout  «  "[parsed  from  circle  file: 

«  circleFlag  «  "  "  «  circleX  «  "  "  «  circleY  «  "  " 

«  circleZ  «  "  "  «  circleRadius  «  "]"  «  endl; 

} 

for  (int  index  =  0;  index  <=  strlen  (circleFlag);  index++)  //uppercase 
circleFlag  [index]  =  toupper  (circleFlag  [index]); 

if  (((strcmp  (circleFlag, "CIRCLE")  ==  0)  II 

(strcmp  (circleFlag, "CYLINDER")  ==  0)  )  && 

(paramcount  ==  5)  &&  (circleRadius  >  0.0)) 

{ 

circleSeparator  =  new  SoSeparator; 
circleTranslate  =  new  SoTranslation; 
circleCylinder  =  new  SoCylinder; 

circleCylinder->parts  =  SoCylinder :: SIDES;  //  let's  see  through* 
allCirclesSeparator->addChild  (  circleSeparator  ) ; 
circleTranslate->translation. setValue (circleX, -circleY, -circleZ) ; 
circleSeparator->addChild  {  circleTranslate  ) ; 
circleSeparator->addChild  (  circleRotation  ) ; 
circleCylinder->radius . setValue (circleRadius)  ; 
circleCylinder->height. setValue (2 .0  *  circleZ); 
circleSeparator->addChild  (  circleCylinder  ) ; 
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} 

else  if  ((strcmp  (circleFlag, "MINE" )  ==  0)  && 

(paramcount  >=  4}) 

{ 

minePairSeparator  =  new  SoSeparator ; 

allCi rclesSeparat:or->addChi Id  {  ininePairSeparator  )  ; 

mineTransla’te  =  new  SoTranslation; 

inineTranslaCe->translation.setValue (circleX, -circleY, -circleZ) ; 
ininePairSeparator->addChild  (  inineTranslate  ); 
mineSeparator  =  readFile  . /viewer/inine^spiked.  iv" )  ; 

^  minePairSeparator->addChild  (  mineSeparator  ); 

else  if  (TRUE)  cout  «  "[failed  parse  test]"  «  endl; 

}  //  end  readCircles 

#endif 

///////////////////////////////////////////////////////////////////////////// 
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in.  EXECUTION  LEVEL  SOURCE  CODE 


This  section  contains  source  code  for  the  Phoenix  execution  level.  Included  files 
are  defines.h,  statevector.h,  statevector.c,  globals.h,  globals.c,  external_functiohs.c, 
parse_functions.c  and  execution.c.  These  files  are  compiled  individually  and  linked 
together  to  form  a  single  executable  program.  These  files  are  available  online  at 
http:llwww.stl.nps.navy  jnill~brutzman/dissertation/software_reference.html 
Files  are  available  individually  or  as  part  of  a  .tar  package  containing  all  Phoenix  AUV  and 
underwater  virtual  world  source  code  and  instructions  on  their  installation  and  use.  The 
execution  level  files  can  be  compiled  and  linked  to  run  on  the  Silicon  Graphics 
workstations  using  the  Makefile  available  at  the  above  location.  To  create  an  executable 
for  the  physical  vehicle,  the  PC  cross  compiler  must  be  used.  This  compiler  requires  that 
statevector.c,  globals.c,  extemal_functions.c  and  parse_functions.c  be  combined  into  a 
single  file  before  compilation  as  follows: 

>  cat  globals.c  statevector.c  extemal_functions.c  parse_functions.c  >  exf.c 
The  files  defines.h,  statevector.h,  globals.h,  exf.c  and  execution.c  can  then  be  moved  to  the 
BURNS  directory  on  the  PC  and  compiled  using  the  linke.bat  batch  file.  The  resulting 
executable  vwll  be  called  executio. 
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Program:  defines. h 


Authors:  Don  Brutzinan  and  Duane  Davis 

Revised:  2  August  96 


System:  AUV  Gespac  68020/68030,  OS-9  version  2.4 

Compiler:  Gespac  cc  Kernighan  &  Richie  (K&R)  C 

Purpose:  Allows  repeated  use  of  global  variables  global. c  via  global. h 

with  one  set  of  defines  in  order  to  prevent  compiler  warnings 

Some  massive  cleanup  will  someday  be  needed  here! 


/******************ic*********i(ic-k*it*icic*itic*-k-kick*-k-k**-k’k*ick-k*i(icic-kici(*-kici(ific**ic**-ki(. 


#ifndef  DEFINES^ 
#define  DEFINES_H 


*  */ 


/*  Figure  out  architecture.  Insert  an  error  to  verify  right  choice  fires.  */ 

#define  os9 
#ifdef  sgi 
#undef  os 9 
#endi f 
#ifdef  sun 
#undef  os 9 
#endif 


/*****************************************************^*^*^^^^*^^^^^^^^^^^^^^ 

/*  Defined  for  Iris  and  OS-9,  located  in  the  same  subdirectories  */ 

#include  <ctype.h> 

#include  <math.h> 

#include  <time.h>. 

/*  Unix  defines  first  */ 

#ifndef  os9 


/*  Irix  defines  in  /usr/include  */ 

/*  sun4  defines  in  /usr/include  */ 

/*  defined  for  Iris  and  OS-9  but  in  different  places  */ 

#include  <stdio.h> 

#include  <sys/types .h> 

#include  <sys/socket . h> 

#include  <netinet/in. h> 

# include  <netdb.h> 

#includG  <signal.h> 

#include  <string.h> 

/*  Unix  library  version  */ 

# include  <errno.h> 

#include  "modes. h" 

#include  "setsys.h" 

#include  "sgstat.h" 
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/*  Now  OS-9  subdirectories  /DEFS,  /DEFS/INET  */ 
#else 


#include  <stdio»h> 

/*  defined  for  Iris  and  OS-9  but  in  different  places  */ 
#include  <types.h> 
iinclude  <inet/socket .h> 

#include  <inet/in.h> 

#include  <inet/netdb.h> 
iinclude  <signal.h> 

/*  OS-9  local  version  (put  it  there)  */ 

# include  "errno.h" 

#include  <inodes.h> 

/*  #include  <setsys.h>  */ 

#include  <sgstat.h> 

/*  see  /hO/DEFS/time.h  */ 

#define  CLOCKS_PER_SEC  CLK_TCK 

#endif 


/*  CAUTION:  the  order  of  the  above  files  is  very  finicky  &  can  cause  errors  */ 
/*  if  changed.  Also  note  additional  dependencies  in  OS-9  makefile.  */ 


/*******************************it*******************ifr***********ilir***********y 

/*  files  and  paths  */ 


#define  AUVINFOFILENAME 
#define  AUVINITFILENAME 
#define  AUVSCRIPTFILENAME 


"mission .info" 
"mission . init" 
"mission . script" 


#ifndef  os9 

#define  AUVORDERS FILENAME 
#define  AUVDATAFILENAME 
#define  AUVTEXTFILENAME 
#define  STIOOODATAFILE 
#define  TARGETDATAFILE 
#else 

#define  AUVORDERS  FILENAME 
#define  AUVDATAFILENAME 
#define  AUVTEXTFILENAME 
#define  STIOOODATAFILE 
#define  TARGETDATAFILE 
#endif 

#define  AUVEMAILFILENAME 
#define  CONTROLCONSTANTSINPUTNAME 
#define  CONTROLCONSTANTSOUTPUTNAME 


"mission . output . orders" 
"mission . output . telemetry" 
"mission . output . l_second" 
"stlOOOdataf ile" 

"targetdataf ile" 

" / r 1 /mi s  s i on . ou  tpu  t . o r de  r s " 
"/rl/mission.output. telemetry" 
"  /  r  1  /mi  s  si  on .  output .  l__second" 
"/rl /stlOOOdataf ile" 

"/rl/ targetdataf ile" 

"mission. output .email" 

"control . constants . input" 
"control .constants. output" 


/*  not  included  in  public  distribution  */ 

#define  EMAILADDRESSFILENAME  "mission_email_addresses" 


/*  function  definitions  */  ^ 

#define  sqr{x)  x*x 


/********************************************** *********^******^^^^^^^^^^^^^^ 
/*  constant  definitions  */ 

#define  TRUE  1 
#define  FALSE  0 


#define  ON  1 
#define  OFF  0 
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#define  YES  1 
ftdefine  NO  0 

idefine  LEFT  -1 
♦define  RIGHT  1 

/*  OS-9  has  PI  in  <math.h>  */ 

♦ifndef  PI 

♦define  PI  3.1415926535897932 

♦endif 

/*  note:  these  pointer  constants  should  be  cast  to  the  right  type  when  used  */ 

♦define  DAC2B_ADDR  0xFFF00040/*  updated  */ 

♦define  DAC_LSB_OFFSET  0x2/*  updated  */ 

♦define  DAC1_ADDR  OxFFFOOOOO/*  updated  */ 

♦define  ADC1_ADDR  (DAC1_ADDR  +  0x11)/*  updated  */ 

♦define  ADC1_MSB  0x0/*  updated  */ 

♦define  ADC1_LSB  0x2/*  updated  */ 

♦define  ADC1_CMD_REG  0x4/*  updated  */ 

♦define  ADC1_STATUS_REG  0x4/*  updated  */ 

♦define  ADCl_BDSy  0x40/*  updated  */ 

♦define  ADC2_ADDR  0xFFF00020/*  updated  */ 

♦define  ADC2_CH_GAIN  0x0/*  updated  */ 

♦define  ADC2_STATUS_REG  0x2/*  updated,  */ 

♦define  ADC2_DATA  0x1/*  updated  */ 

♦define  ADC2_CMD_REG  0x2/*  updated  */ 

/*  watch  out  for  potential  conflict  between  DACS  &  input  port  /P  !!!!!!!  */ 

/*  next  2:  updated  */ 

♦define  VIAO_ADDROxFFF00080  /*  PIA  Card  Base  Address  VIAO  Port  */ 

♦define  VIA1_ADDR (VIA0_ADDR  +  0x20)/*  VIAl  Port  */ 


♦define  ORB_IRB  1/*  updated  */ 
♦define  ORA_IRA  3/*  updated  */ 
♦define  DDRB  5/*  updated  */ 
♦define  DDRA  7/*  updated  */ 


♦define  T1C_L  4 

♦define  T1C_H  5 

♦define  T1L_L  6 

♦define  T1L_H  .  7 

♦define  T2C_L  8 

♦define  T2C_H  9 

♦define  ACR  11 

♦define  PCR  12 

♦define  IFR  13 

♦define  lER  14 

♦define  SONAR_SWl  OxOE 

♦define  SONAR_SW2  OxOD 

♦define  SONAR_SW3  OxOB 

♦define  SONAR_SW4  0x07 

♦define  SONAR_TRIGl  0x10 

♦define  SONAR_TRIG2  0x20 


♦define  AVG_PTS  10 

♦define  MAX_RNG_DIFF  22.0 

♦define  MIN_NO_PTS  3 

♦define  MAX_BAD_PTS  3 


/*  verification  required  */ 
♦define  BOW_RUDDER_TOP  1 

♦define  BOW_RUDDER_BOTTOM  1 


♦define  BOW_PLANE_STBD  3 

♦define  BOW_PLANE_PORT  3 

♦define  STERN_RUDDER_TOP  2 

♦define  STERN_RUDDER_BOTTOM  2 
♦define  STERN_PLANE_STBD  4 

♦define  STERN_PLANE_PORT  4 

♦define  CONVERT_TO_FEET 

♦define  RIGHTJSIOTOR 
♦define  LEFT_MOTOR 
♦define  SUPPLY 

♦define  RIGHT_MOTOR_RPM 
♦define  LEFT_MOTOR_RPM 

/*  For  ADC2  Card  */ 

♦define  ROLL_ANGLE_CH 
♦define  PITCH_ANGLE_CH 
♦define  ROLL_RATE_CH 
♦define  PITCH_RATE_CH 
♦define  YAW_RATE_CH 
♦define  DEPTH_CELL_CH 
♦define  DOWN_SONAR_CH 

/*  For  ADAl  Card  */ 

♦define  COMPUTER_VOLTAGE_CH  7/*  updated  */ 

♦define  MOTOR_GYRO_VOLTAGE_CH  8/*  updated  */ 


♦define  MFI_BASE  (0xFFF00700) /*  updated  */ 

/*  parallel  port  defines  are  frommfi_a3.c  */ 

/*  this  is  the  board  base  address  on  the  G96  bus  */ 

♦define  MFI_INPUT_PORT  0 
♦define  MFI_OUTPUT_PORT  1 


0.02398 

0 

2 

1 

0  . 

1 


12/*  updated  */ 
11/*  updated  */ 

9  /*  updated  */ 
8/*  updated  */ 
10/*  updated  */ 
7/*  updated  */ 
14/*  updated  */ 


/*  One  stream  socket  is  used  with  adequate  throughput  */ 

/*  (although  two  could  work,  no  performance  improvement  is  expected) 


/*  Be  careful  that  you  reserve  these  port  numbers  to  prevent  collisions 
*/ 

/*  from  other  processes  requesting  ports  on  your  system:  */ 


♦define  DISBRIDGE_TCP_PORT 
*/ 

♦define  NPSNETJMCAST_PORT 
*/ 


♦define  AUVSIM1_TCP_PORT_0 
*/ 

♦define  AUVSIM1_TCP_P0RT_1 
*/ 

♦define  AUVSIM1_TCP_P0RT_2 
try)*/ 

♦define  AUVSIM1_TCP_P0RT_3 
*/ 

♦define  AUVSIM1_TCP_P0RT_4 
♦define  AUVSIM1_TCP_P0RT_5 
♦define  AUVSIMl_TCP_PORT_6 
♦define  AUVSIM1_TCP_P0RT_7 
♦define  AUVSIM1_TCP_P0RT_8 


2056  /*  disbridge  1.3  program,  server  &  client 


3111  /*  Mike  Macedonia's  multicast  DIS  2.0.3 
/*  NPS  Autonomous  Underwater  Vehicle  (AUV) */ 


/ 

* 

Underwater  Virtual  World 

(UVW)  *, 

3210 

/* 

os9sender  <==>  os9server  test  programs 

3211 

/* 

auv 

execution  level  <==>  virtual  world 

3212 

/* 

auv 

execution  <==>  tactical 

(teleme- 

3213 

/* 

auv 

execution  <==>  tactical 

(messages) 

3214 

/* 

port  for  future  use 

*/ 

3215 

/* 

port  for  future  use 

*/ 

3216 

/* 

port  for  future  use 

*/ 

3217 

/* 

port  for  future,  use 

*/ 

3218 

/* 

port  for  future  use 

*/ 
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♦define  AUVSIM1_TCP_P0RT_9  3219  /* 
♦define  SOCKET_QUEUE_SIZE  5  /* 

♦define  VIRTUAL_WORLD_REMOTE_HOST_NAME 
♦define  TACTICAL_REMOTE_HOST_NAME 
♦define  MAXBUFPERSIZE  500  /* 

/*************♦***********************• 
/*  New  constants 

♦define  FILEBUFFERSIZE  10 

♦define  PORT_PROP  0 
♦define  STBD_PROP  1 
♦define  BOW_VERTICAL  2 
♦define  STERN_VERTICAL  4 
♦define  BOW_LATERAL  3 
♦define  STERN_LATERAL  5 


port  for  future  use  */ 

max  allowed  by  TCP/IP  */ 

"fletch.stl.nps.navy.mil" 

" f letch. St 1 .nps.navy.mil" 

value  >  255  may  break  serial  port  «<*/ 

r***********************:k*************/ 

*/ 


/***************************************************************************^ 
/*  units  in  feet,  these  values  are  also  :{  in  -/c^namics/AWglobals.H  */ 
/*  top  aft  */ 

♦define  AUV_ST725_x_offset  2.625 
♦define  AUV_ST725_y_offset  -0.16666667 
♦define  AUV_ST725_z_offset  -0.33333333 


/*  bottom  forward  */ 

♦define  AUV_ST1000_x_of fset  2.875 
♦define  AUV_ST1000_y_of fset  -0.16666667 
♦define  AUV_ST1000_z_of fset  0.33333333 


/****************  ***1,  *1,  ****  i,**„*i,t***  ********  *i,.i,*i,*i,**i,i,*t***  ******  *********/ 

/*  marco  exec.h  -  verify  and  clean  up  «««««««<«  */ 

♦define  TIM_1AC_1  0xFFF00610  /*  Timer  Card  Base  Addresses  */ 

♦define  TIM_1AC_2  OxFFF00620 

♦define  TIM_1AC_3  0xFFF00640 

♦define  TIM_1AC_DATA_REG  0x5 

♦define  TIM_1AC_C0NTR0L_REG  0x7 
♦define  TIM_1AC_AUX_GATES_REG  0x9 


♦endif 

/*  DEFINES_H  */ 


66 


/*  *******/ 
Program:  statevector . h 


Authors : 
Revi sed : 


State  vector  {telemetry  variables)  common  definition 
Don  Brutzman,  Mike  Burns  and  Duane  Davis 
2  May  96 


System: 

Compiler: 


AUV  Gespac  68020/68030,  OS-9  version  2.4 
Gespac  cc  Kernighan  &  Richie  (K&R)  C 


Compilation: 

[68020] 

[68030] 


ftp>  put  statevector . c 
auvsiml>  chd  execution 
auvsiml>  make  -k2f  execution 
auvsiml>  make  execution 


[Irix 


fletch>  make  execution 


Purpose:  Allows  repeated  use  of  global  variables  in  statevector . c 

via  statevector .h  in  order  to  prevent  compiler  warnings 

See  globals.c/globals.h  for  other  global  variables 

Religion:  All  distance  units  are  feet,  all  time  units  are  seconds, 

all  rotational  units  are  degrees.  This  is  only  required 
when  transmitting  values  externally  (socket/text/file) . 

Deciding  factors  are  consistency  and  human  readability.. 
Computational  performance  is  not  an  issue. 


Anyone  who  disagrees  has  to  put  up  with  an  endless  argument 
from  Don  who  will  not  be  persuaded  to  accept  any  variations! 

*/ 

/************icicic*ic***icic*icic***-kic*ic**ic***ic-ic:kic**ic**-k-k-kic**icici:ic***ic*ici:**icic*icic****/ 


#include  "defines. h" 


#ifndef  STATEVECTOR_H  ^ 

#define  STATEVECTOR^H 


extern  int 


STATEVECTORSIZE  ;  /*  how  many  variables  */ 


extern  char 


keyword  [300] 


/*  auv_state  or  uvw__state  */ 


extern  double 
extern  double 
extern  double 
extern  double 
extern  double 
extern  double 
extern  double 
extern  double 
extern  double 
extern  double 
extern  double 
extern  double 
extern  double 
extern  double 

extern  double 
extern  double 
extern  double 


t 

; 

/* 

units  are  seconds  */ 

X 

; 

/* 

feet 

V 

y 

t 

/* 

feet 

*/ 

z 

i 

/* 

feet 

*/ 

phi 

/* 

degrees 

*/ 

theta 

i 

/* 

degrees 

*/ 

psi 

/ 

/* 

degrees 

*/ 

x_dot 

7 

/* 

feet/sec 

*/ 

y_dot 

7 

/* 

feet/sec 

*/ 

z_dot 

7 

/* 

feet/sec 

*/ 

phi_dot 

7 

/* 

degrees /sec 

*/ 

theta_dot 

7 

/* 

degrees/ sec 

*/ 

psi_dot 

7 

/* 

degrees/sec 

*/ 

speed 

;  / 

*  feet/sec  (paddlewheel ) 

/ 

* 

possibly  averaged 

u 

7 

/* 

feet/sec 

V 

V 

; 

/* 

feet/sec 

*/ 

w 

7 

/* 

feet/sec 

*/ 
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extern 

double 

P 

;  /* 

degrees/sec 

*/ 

extern 

double 

q 

;  /* 

degrees/sec 

*/ 

extern 

double 

r 

;  /* 

degrees/sec 

*/ 

extern 

double 

del ta_planes 

;  /* 

degrees 

*/  • 

extern 

double 

delta_rudder 

;  /* 

degrees 

*/ 

extern 

double 

port_rpm 

;  /* 

-700. ,700 

*/ 

extern 

double 

s  tbd_rpm 

;  /* 

-700 . .700 

*/ 

/*  +-  24V  <=> 

+  -2  lb,  +  Volts  moves 

thruster  in 

+  direction,  all 

identical 

extern 

double 

AUV_bow_vertical  ; 

/* 

thruster 

rpm 

*/ 

extern 

double 

AUV_stern_vertical  ; 

/* 

thruster 

rpm 

*/ 

extern 

double 

AUV_bow_l a  t e ra 1  ; 

/* 

thruster 

rpm 

*/ 

extern 

double 

AUV_stern__lateral  ; 

/* 

thruster 

rpm 

*/ 

/*  warning:  do  not  use  leading  zero  with  bearings  or  else  read  as  octal  */ 
extern  double  AUV_ST1000_bearing  ;  /*  ST_1000  conical  bearing  degrees*/ 

extern  double  AUV_ST1000_range  ;  /*  ST_1000  conical  range  feet  */ 

extern  double  AUV_ST1000_strength;  /*  ST_1000  conical  strength  dB  */ 

extern  double  AXJV_ST725_bearing  ;  /*  ST_725  1  x  24  sector  bearing  degrees*/ 
extern  double  AUV_ST7 2 Strange  ;  /*  ST_725  1  x  24  sector  range  feet  */ 

extern  double  AUV_ST72  5_strength  ;  /*  ST_725  1  x  24  sector  strength  dB  */ 

extern  double  divetrac]cer_r angel;  /*  fe.et  range  to  divetracker  unit  1  */ 

extern  double  divetracker_range2;  /*  feet  range  to  divetracker  unit  2  */ 

/*  negative  range  means  invalid  return  */ 
/*  future:  divetracker_headingl  &  2  */ 


#endif 

/*  end  statevector.h  */ 
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/* 

Program: 


Authors : 
Revised: 


statevector .c 

State  vector  (telemetry  variables)  common  definition 
Don  Brutzman,  Mike  Burns  and  Duane  Davis 
9  June  96 


System: 

Compiler: 

Compilation: 

[68020] 

[68030] 

[Irix  ] 

Purpose: 


Religion: 


*/ 


AUV  Gespac  68020/68030,  OS-9  version  2.4 
Gespac  cc  Kernighan  &  Richie  (K&R)  C 

ftp>  put  statevector . c 
auvsiml>  chd  execution 
auvsiml>  make  -k2f  execution 
auvsiml>  make  execution 

fletch>  make  execution 


Allows  repeated  use  of  global  variables  in  statevector . c 
via  statevector .h  in  order  to  prevent  compiler  warnings 

See  globals.c/globals.h  for  other  global  variables 

All  distance  units  are  feet,  all  time  units  are  seconds, 
all  rotational  units  are  degrees.  This  is  only  required 
when  transmitting  values  externally  (socket/text/file) .  . 

Deciding  factors  are  consistency  and  human  readability. 
Computational  performance  is  not  an  issue. 

Anyone  who  disagrees  has  to  put  up  with  an  endless  argument 
from  Don  who  will  not  be  persuaded  to  accept  any  variations' 


#include  "defines. h" 


int 

char 


STATEVECTORSIZE  =  37;  /*  how  many  variables  follow*/ 

keyword  [300];  /*  auv_state  or  uvw_state  */ 


double 

double 

double 

double 

double 

double 

double 

double 

double 

double 

double 

double 

double 

double 


t 

X 

Y 

2 

phi 

theta 

psi 

x_dot 

y_dot 

z__dot 

phi_dot 

theta_dot 

psi_dot 

speed 


double  u 
double  v 
double  w 
double  p 
double  q 
double  r 


=  0.0;  /*  units  are  seconds  */ 


=5.0;  /*  feet  */ 
=  5.0;  /*  feet  */ 
,=2.0;  /*  feet  */ 
=  0.0;  /*  degrees  */ 
=  0.0;  /*  degrees  */ 
=0.0;  /*  degrees  */ 
=  0.0;  /*  feet/sec  */ 
=0.0;  /*  feet /sec  */ 
=0.0;  /*  feet /sec  */ 


=  0.0;  /*  degrees/sec  */ 

=  0.0;  /*  degrees/sec  */ 

=  0.0;  /*  degrees/sec  */ 

=0.0;  /*  feet /sec  (paddlewheel )  */ 
/*  possibly  averaged  */ 

=  0.0;  /*  feet/sec  */ 

=  0.0;  /*  feet/sec  */ 

=  0.0;  /*  feet/sec  */ 

=0.0;  /*  degrees/sec  */ 

=0.0;  /*  degrees/sec  */ 

=  0.0;  /*  degrees/sec  */ 


double  delta_planes  =  0.0;  /*  degrees 

double  delta^rudder  =  0.0;  /*  degrees  */ 

double  port_rpin  =  0  ;  /*  -700.. 700  */ 

double  stbd_rpm  =0  ;  /*  -700.. 700  */ 


/*  +-  24V  <=>  +-2  lb,  +  Volts  moves  thruster  in  +  direction,  all  identical*/ 
double  AUV_bow__vertical  =0.0;  /*  thruster  rpm  */ 
double  AUV_stern_vertical  =  0.0;  /*  thruster  rpm  */ 
double  AUV_bow_lateral  =  0.0;  /*  thruster  rpm  */ 
double  AUV_stern_lateral  =  0.0;  /*  thruster  rpm  */ 

/*  warning:  do  not  use  leading  zero  with  bearings  or  else  read  as  octal  */ 

double  AUV_ST1000_bearing  =  0.0;/*  ST_1000  conical  bearing  degrees  */ 
double  AUV_ST1000_range  =  0.0;/*  ST_1000  conical  range  feet  */ 
double  AUV_ST1000_strength=  -1.0;/*  ST_1000  conical  strength  dB  */ 

double  AUV_ST725_bearing  =  0.0;/*  ST_725  1  x  24  sector  bearing  degrees*/ 
double  AUV_ST725_range  =  0.0;/*  ST_725  1  x  24  sector  range  feet  */ 
double  AUV_ST725_strength  =  -1.0;/*  ST_725  1  x  24  sector  strength  dB  */ 

double  divetracker_rangel  =  -1.0;/*  feet  range  to  divetracker  unit  1  */ 
double  divetracker_range2  =  -1.0;/*  feet  range  to  divetracker  unit  2  */ 

/*  negative  range  means  invalid  return  */ 
/*  future:  divetracker_headingl  &  2  */ 


/*  end  statevector.h  */ 
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/★************★******************** *****************************************^ 
/* 

Program:  globals .h 

Authors:  Don  Brutzman,  Duane  Davis 

Revised:  2  August  96 

System:  AUV  Gespac  68020/68030,  OS'-9  version  2.4 

Compiler:  Gespac  cc  Kernighan  &  Richie  (K&R)  C 

Compilation:  ftp>  put  globals.h 

auvsiml>  chd  execution 
[68020]  auvsiml>  make  -k2f  execution 

[68030]  auvsiml>  make  execution 

[Irix  ]  fletch>  make  execution 

Purpose:  Allows  repeated  use  of  global  variables  global. c  via  global. h 

in  order  to  prevent  compiler  warnings 

See  statevector .c/statevector .h  for  other  global  variables 

*/ 

/***************************************************************************/ 
tifndef  GLOBALSJ 
#define  GLOBALS_H 

/**★**********★************************************************************* ^ 
tinclude  "defines. h" 

#ifdef  os9 

/*  these  are  marco  routines  for  divetracker  */ 

#include  "dt2cl.h" 

# include  "modul o . h" 

#endif 


/*  Program  configuration  flags  / 


extern 

int 

TRACE 

;  /* 

l=trace  on,  0=trace  off 

*/ 

extern 

int 

DISPLAYSCREEN 

;  /* 

l=screen  on,  0=screen  off 

*/ 

extern 

int 

LOCATIONLAB 

;  /* 

l=:virtual  world,  0=actual  vehicle 

*/ 

extern 

int 

BENCHTEST 

;  /* 

Isvirtual  world, 0=actual  vehicle 

*/ 

extern 

int 

TACTICAL 

;  /* 

l=tactical  on,  0=tactical  off 

V 

extern 

int 

LOOPFOREVER 

;  /* 

l=repeat  execution  indefinitely 

*/ 

extern 

int 

LOOPFILEBACKUP 

;  /* 

l=backup  files  between  replications 

:V 

extern 

int 

PARALLELPORTTRACE 

;  /* 

l=trace  each  char  received  at  port 

*/ 

extern 

int 

SONARINSTALLED 

;  /* 

l=sonar  head  available  for  query 

*/ 

extern 

int 

SONARTRACE 

;  /* 

l=trace  on,  0=trace  off 

*/ 

extern 

double 

SONARHEADINGSTEP 

;  /* 

degrees- 

*/ 

extern 

int 

SONARSCANMODE 

;  /* 

l=forward,  0  =  target  tracking 

*/ 

extern 

int 

SONARSCANDIRECTION 

;  /* 

l=right,  2  =  left 

*/ 

extern 

double 

s  1 1 0  0  0__c  oitimand 

;  /* 

degrees  commanded  bearing 

*/ 

extern 

int 

ENTERCONTROLCONSTANTS 

;  /* 

1  =  manual  entry,  0=default  values 

*/ 

extern 

int 

SHOWCONTROLCONSTANTS 

;  /* 

1  =  print  constants,  0=default 

*/ 

extern 

int 

LOADCONTROLCONSTANTS 

;  /* 

1  =  file  entry,  0=default  values 

*/ 

extern 

int 

REALTIME 

;  /* 

1  =  real-time  waits,  0  =  no-pause 

*/ 

extern 

int 

DIVETRACKER 

;  /* 

l=no  dive  tracker  means  abort 

*/ 

extern 

int 

DEADRECKON 

;  /* 

l=dead  reckon  navigate,  0=regular 

V 
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extern  int  DEADSTICKRUDDER  ;  /*  l=use  ordered  rudder,  0  =  control  */ 
extern  int  DEADSTICKPLANES  ;  /*  l=use  ordered  planes,  0  =  control  */ 
extern  int  INTEGRALDEPTHCONTROL  ;  /*  l=use  PID,  0  =  use  PD  */ 
extern  double  tiine_int„control_on  ;  /*  give  PD  a  chance  to  get  close  */ 

/*  untested  */ 

extern  int  SLIDINGMODECOURSE  ;  /*  l=use  sliding  mode,  0  =  control  */ 

extern  int  THRUSTERCONTROL  ;  /*  l=use  thrusters,  0=use  propellers  */ 
extern  int  ROTATECONTROL  ;  /*  l=use  thrusters  to  rotate  in  place  */ 
extern  int  IxATERALCONTROL  ;  /*  l=use  thrusters  for  lateral  motion*/ 


extern  int  FOLLOWWAYPOINTMODE  ;  /*  1=  go  to  WAYPOINT  without  WAITS  */ 

extern  int  WAYPOINTCONTROL  ;  /*  1=  go  to  WAYPOINT  */ 

extern  int  HOVERCONTROL  ;  /*  l=hover  at  WAYPOINT  */ 

extern  int  TARGETCONTROL  ;  /*  l=hover  relative  to  a  target  */ 

extern  int  NEWTARGET  ;  /*  l=target  is  new,  0=:target  is  old  */ 

extern  int  TARGETPOINTING  ;  /*  l=:point  at  target  if  TARGETCONTROL  */ 

extern  int  TARGETEDGETRACK  ;  /*  l=trac]cing  on  target  edge  only  */ 

extern  int  NEWTARGETSTATION  ;  /*  l=new  station  keeping- point  */ 

extern  int  RECOVERYCONTROL  ;  /*  l=recovery  in  progress  */ 

extern  int  NEWRECOVERYCOMMAND  ;  /*  l=command  new,  0=in  progress  */ 

extern  double  SCANWIDTH  ;  /*  degrees  full  scan,  centered  on  bow  */ 

extern  int  LEAK  ;  /*  l=water  leak  in  progress  */ 

extern  int  HALTSCRIPT  ;  /*  l=automatic  shutdown  criteria  met  */ 

extern  int  DEATH_SPIRAL_RESET  ;  /*  l=reset  death  spiral  checker  */ 


#ifndef  os9 
extern  int 
#else 

extern  int 
#endif 

extern  int  EMAIL_ENTERED  ;  /*  flag  for  first  time  through  */ 

extern  int  NOT_YET_RE IMPLEMENTED  ;  /*  code  in  block  needs  reverification 

*/ 

extern  int  ARCHAIC_IGNORE  ;  /*  code  in  block  not  valid,  commented  */ 

extern  double  TIMESTEP  ;  /*  time  of  a  single  closed  loop  */ 

/*  add  code  to  warn  if  exceeded  ««  */ 

/*  units  are  seconds  */ 

extern  int  TACTICALPARSE  ;  /*  l=tactical  level  parsing  commands  */ 

extern  int  KEYBOARDINPUT  ;  /*  l=read  keyboard  vice  mission  file  */ 

extern  int  HELPFILELAUNCHED  ;  /*  l=mission . script  -  HELP  already  shown*/ 

extern  int  GPSFIXINPROGRESS  ;  /*  l=wait  GPS-FIX  &  restore  z^command  */ 

extern  int  REPORTSTABLE  ;  /*  l=tell  when  stable  hover/waypt/gps  */ 

extern  int  REPLAY  ;  /*  l=replay  of  existing  telemetry  file  */ 

extern  int  NOSCRIPT  ;  /*  l=:replay  of  existing  telemetry  file  */ 

/*  with  no  accompanying  script  file  */ 

/ 

**Hr*************************************************************************/ 
/*  files  and  paths  */ 

extern  FILE  *  auvscriptf ile; 

extern  FILE  *  auvordersf ile; 

extern  FILE  *  auvdatafile; 

extern  FILE  *  auvtextfile; 

extern  FILE  *  telemetry_f ile; 

extern  FILE  *  controlconstantsinputf ile; 

extern  FILE  *  control constantsoutputf ile; 


EMAIL  ;  /*  l=send  e-mail,  0=don't  send  e-mail  */ 

EMAIL  ;  /*  can't  send  email  via  OS -9  directly  */ 
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extern  FILE  *  emailaddressf ile; 
extern  FILE  *  stlOOOdataf ile; 
extern  FILE  *  targetdataf ile; 

/*  FILE  *  serialtestfile;  */ 


/*  located  here  due  to  gespac  cross-compiler  */ 
extern  char  TELEMETRYFILENAME  [255]; 


extern  int  serialpath  ; 

extern  int  sonarpath 


/ 

*****************  ************************************************** ******^^^^ 
/*  Variables  and  data  structures  */ 

/*  buffers  of  full  strings  for  byte  transfer  to  tactical  level  &  disk  file  */ 
/*  'buffer'  usually  <  256,  intentionally  oversized  in  case  of  overflow  error*/ 

extern  time_t  systein__time 

extern  struct  tm  *systein_tmp  ; 

/*  partial  structure  template  for  the  MFI  (only  interested  in  PIA  for  now)*/ 


struct  MFI_PIA 
{ 

unsigned  short  pra; 
unsigned  short  era; 
unsigned  short  prb; 
unsigned  short  erb; 
}; 


/*  port  register  A  -  data  direction  A  */ 
/*  control  register  A  */ 
/*  port  register  B  -  data  direction  B  */ 
/*  control  register  B  */ 


/*  dac:  digital-analog  converter 
/*  adc:  analog-digital  converter 

/*  4  Channels  of  DAC  ADA-1  DAC 

extern  unsigned  char  *dacl_a 

/*  8  Channels  of  DAC  DAC-2B 

extern  unsigned  char  *dac2b_a  ; 


*/ 

*/ 


*/ 


*/ 


/*  16  Channels  of  ADC  ADA-1 
extern  unsigned  char  *adcl_a  ; 

/*  16  Channels  of  ADC  ADC-2 
extern  unsigned  short  *adc2_a  ; 

extern  unsigned  char  *viaO  ; 
extern  unsigned  char  *vial  ; 


extern  unsigned  char  viaOa_reg,  viaOb_reg; 


extern  int 
extern  int 
extern  int 
extern  int 
extern  int 


telemetry_records_saved  ; 
mission_leg_counter  ; 
replication_count  ; 
end_test  ; 
wrap_count  ; 


extern  double  dt 

extern  double  rpm 


/*  units  are  seconds  */ 
/*  -700. .700  */ 


extern  double 


dt_time 


/*  dive  tracker  time  */ 


extern  double 
extern  double 


computer_voltage 

motor_voltage 


73 


extern  double 

vertical_thruster_volts  ; /*intermediate 

calculation 

extern  double 

lateral_thruster_volts 

;  /*  intermediate  calculation 

extern  double 

ma  i  n__mo  t  o  r_de  1 1  a  1 

/ 

extern  double 

main_motor_del ta2 

/ 

extern  int 

mai n_mo t 0 r_vo 1 t 1 

7 

extern  int 

main_motor_volt2 

/ 

extern  unsigned 

short  psi_bit_old 

/ 

extern  double 

dg__of  fset 

7 

extern  double 

startups! 

7 

/*  Used  to  estimate  the  X  and  Y  position  of 

the  AUV  */ 

extern  double 

X_est 

/ 

extern  double 

Y_est 

/ 

extern  double 

X_dot_est 

; 

extern  double 

Y_dot_est 

/ 

extern  double 

u_est 

/ 

extern  double 

v_est 

/ 

/*  control  coefficients  are  based  on  standard 

units  (degrees/feet/seconds) 

extern  double 

k_psi 

7 

extern  double 

k  r 

7 

extern  double 

k_v 

/ 

extern  double 

k_z 

/ 

extern  double 

k_w 

7 

extern  double 

k_theta 

7 

extern  double 

k_q 

'* 

extern  double 

k_thruster_psi 

7 

extern  double 

k_thruster_r 

/ 

extern  double 

k_thruster_rotate 

7 

extern  double 

k_thruster_lateral 

7 

extern  double 

k_thruster_2 

/ 

extern  double 

k_thruster_w 

7 

extern  double 

k_thruster_theta 

7 

extern  double 

k_p  rope  1 1  er_hove  r 

7 

extern  double 

k_surge_hover 

/ 

extern  double 

k_p  rope  1 1  er_cur  rent 

7 

extern  double 

k_t  h  r  u  s  t  e  r_ho  ve  r 

; 

extern  double 

k_s  way  __ho  ve  r 

7 

extern  double 

k_thruster_current 

7 

extern  double ’ 

k_sigma_r 

7 

extern  double 

k_sigma_psi 

/ 

extern  double 

eta_steering 

7 

extern  double 

sigma 

7 

extern  int 

mission_legs_total 

/ 

/*  values 

initialized  in  parse__mission_script_commands 

0  */ 

extern  double 

t  ime_next_command 

;  /*  units  are 

seconds  */ 

extern  double 

t  ime_gps_comp  le  te 

;  /*  units  are 

seconds  */ 

extern  double 

t  ime_p  0  s  t  gp  s_di  ve 

;  /*  units  are 

seconds  */ 

extern  double 

psi^command 

;  /*  degrees 

*/ 

extern  double 

psi_coimnand_hover  ;  /*  degrees 

*/ 

extern  double 

t  he  t  a_c  ommand 

;  /*  degrees 

extern  double 

x_command 

;  /*  feet 

*/ 

extern  double 

y_c  ommand 

;  /*  feet 

*/ 

extern  double 

2_c  ommand 

;  /*  feet 

*/ 

extern  double 

s  t  bd__rpm_c  ommand 

;  /*  -700. .700 

*/ 

extern  double 

po  r  t_rpm_c  omma  nd 

;  /*  -700. .700 

*/ 

extern  double 

planes_c  ommand 

;  /*  degrees 

*/ 
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extern 

double 

rudder_command  ; 

/* 

degrees 

*/ 

extern 

double 

rotate_command  ; 

/* 

degrees/sec 

*/ 

extern 

double 

1 a t er a l_c omma nd 

/* 

ft/sec 

*/ 

extern 

double 

bow_lateral_thruster_command  ; 

/* 

volts  -24 . . 

24  */ 

extern 

double 

stern_lateral_thruster_command  ; 

/* 

volts  -24 . . 

24  */ 

extern 

double 

bow_vertical_thruster_corfm\and  ; 

/* 

volts  -24 . . 

24  */ 

extern 

double 

stern_vertical_thruster_cominand  ; 

/* 

volts  -24 . . 

24  */ 

extern 

double 

previ  ous_z_coininand 

/* 

feet 

*/ 

extern 

double 

r  a  nge_f  r  oin_r  e  c  o  ve  ry  _p  t  ; 

/* 

feet 

*/ 

extern 

double 

gyro_error 

/* 

degrees 

*/ 

extern 

double 

way point_di stance  ; 

/* 

feet 

*/ 

extern 

doubl e 

waypoint_angle  ; 

/* 

degrees 

*/ 

extern 

double 

track_angle  ; 

/* 

degrees 

*/ 

extern 

double 

along_t racked! stance  ; 

/* 

feet 

*/ 

extern 

double 

cross_track_distance  ; 

/* 

feet 

*/ 

extern 

double 

standoff_di stance  ; 

/* 

feet 

*/ 

extern 

double 

dea  th_spi ral_radius  ; 

/* 

feet 

*/ 

extern 

double 

depth^error  ; 

/* 

feet 

*/ 

extern 

double 

depth_cell_bias  ; 

/* 

feet 

extern 

double 

psi_error  ; 

/* 

degrees 

*/ 

/*  parameters 
extern  double 

required  for  targetcontrol  */ 

targe t_x  ; 

/* 

feet,  world 

coords 

*/ 

extern 

doubl e 

targe t_y 

/* 

feet,  world 

coords 

*/ 

extern 

double 

targe t_z  ; 

/* 

feet,  world 

coords 

*/ 

extern 

double 

target_bearing  ; 

/* 

degrees 

*/ 

extern 

double 

targe t_bearing_command  ; 

/* 

degrees 

*/ 

extern 

double 

t  a  r ge  t_be  a  r i ng_do  t  ; 

/* 

degrees  per 

second 

*/ 

extern 

double 

targe t_range  ; 

/* 

feet 

V 

extern 

double 

targe  t__range_command  ; 

/* 

feet 

*/ 

extern 

double 

target_range_dot  ; 

/* 

feet  per  second 

*/ 

/*  psi 

i“l  for 

differentiation  of  r  needed  because  of  busted  gyro  */ 

extern 

double 

psi_iml  ; 

/* 

degrees 

*/ 

/*  values  used  by  kahlman  depth  filter  */ 
extern  int  ,  kal_init_z 

extern  double  thres_z 

extern  double  z_kal 

extern  double  z_dot_kal 

extern  double  z_ddot_kal 

extern  int 
extern  int 
extern  int 
extern  int 
extern  int 
extern  int 
extern  int 
extern  int 
extern  int 
extern  int 
extern  int 
extern  int 
extern  double 
extern  double 
extern  double 
extern  double 
extern  double 
extern  int 
extern  int 


roll_rate_0 

pitch_rate_0 

yaw_rate_0 

roll_0 

pitch_0 

z_valO 

swl 

error 

range 

bad_rng 

bad_updates 

range_index 

range 1 

range 2 

errorl 

error2 

avg_rng 

k_range 

range_a  r r ay  (3000)  ; 
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extern 

int 

speed__array  [  11  ] 

7 

extern 

int 

PortAFlag 

7 

extern 

int 

tick 

7 

extern 

int 

curr_tick 

7 

extern 

int 

tickl 

7 

extern 

int 

tick2 

7 

extern 

int 

i 

7 

extern 

int 

mask 

/ 

extern 

long 

davedate 

7 

extern 

long 

davetime 

; 

extern 

double 

value 

7 

extern 

short 

day 

7 

extern 

char 

answer 

7 

extern 

int 

start_dwell 

/ 

extern 

int 

socket_descriptor 

/ 

extern 

int 

socket_accepted 

/ 

extern 

int 

socket_stream 

7 

extern 

int 

tactical_socket_descriptor; 

extern 

int 

tactical_socket_accepted  ; 

extern 

int 

tactical_socket_stream 

7 

extern 

int 

socket_length  ; 

/* 

extern 

int 

bytes_received 

7 

extern 

int 

bytes_read 

7 

extern 

int 

bytes_written 

7 

extern 

int 

bytes_left 

7 

extern 

int 

bytes_sent 

7 

max  allowed  packet  size*/ 


/*  char  t)uffer_array  [FILEBUFFERSIZE]  [256] ;  --  not  implemented 


extern  char 
extern  char 
extern  int 
extern  int 
extern  int 
extern  int 

extern  char 


extern  FILE 


buffer  [MAXBUFFERSIZE  +  10]; 

local_buffer  [MAXBUFFERSIZE  +  10]; 

buffer_size  ; 

buffer^max  ; 

buffer_index 

variablesjarsed  ; 

buf fer_received  [MAXBUFFERSIZE  +  10], 

virtual_world_remote_host_name  [60] , 

.  tactical_remote_host_name  [60], 

command_buf fer  [MAXBUFFERSIZE  +  10]; 

*  netstat_f ileptr; 


extern  struct  sockaddr_in  serve r_address; 


*/ 


extern  struct  hostent  *server_entity ; 


extern  char 


ema i  l_addre s s  [81]; 


extern  int 
extern  int 
extern  int 


shutdown_signal_received 
virtual_world_socket_opened  ; 
tactical_socket_opened  ; 


extern  char  *  ptr_index; 


extern  int  print_help; 


extern  double 


speed_jper_rpm;/*  steady  state:  2.0  feet/sec  per  700  rpm  */ 
/*  -700.  .700  */ 
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extern 

clocks 

t  nextloopclock; 

extern 

clocks 

t  currentloopclock; 

extern 

int 

audible_corninand 

; 

extern 

int 

auvscriptf ilequit 

/ 

extern 

double 

AUV_oceancurrent_x; 

/* 

extern 

double 

AUV_oceancurrent_y ; 

/* 

extern 

double 

AUV_oc€ancurrent_z ; 

/* 

extern 

double 

DiveTrackerl_x ; 

/* 

extern 

double 

DiveTrackerl_y  ; 

/* 

extern 

double 

DiveTrack€rl_z ; 

/* 

extern 

double 

Di veTracker2_x ; 

/* 

extern 

double 

DiveTracker2_y ; 

/* 

extern 

double 

DiveTracker2_z ; 

/* 

Ocean  current  rate  along  North-axis  */ 
Ocean  current  rate  along  East-axis  */ 
Ocean  current  rate  along  Depth-axis  */ 

DiveTrackerl  transducer  x  (feet)  */ 
DiveTrackerl  transducer  y  (feet)  */ 
DiveTrackerl  transducer  z  (feet)  */ 

DiveTracker2  transducer  x  (feet)  */ 
DiveTracker2  transducer  y  (feet)  */ 
DiveTrack€r2  transducer  z  (feet)  */ 

r************************************^ 


/*  Dave's  cats  and  dogs  */ 

extern  unsigned  char  *tiin_lacl; 

extern  unsigned  char  *tiiTUlac2; 

extern  unsigned  char  *tiin_lac3; 

extern  unsigned  char  tiiTt_la_data_reg 

extern  unsigned  char  tim_la_control_reg  ; 

extern  unsigned  char  tiin_la_aux_gates_reg  ; 

/*  Dive  Tracker  Process  Stuff  */ 

tifdef  os9 

extern  int  os9forkc(); 
extern  char  **environ; 

extern  char  *dt_fork_pannptr; 
extern  char  *argblk[]; 
extern  int  dt_pid; 
extern  int  ul_^id; 

#endif 

/***★*********************★*****************★*******************************/ 

#endif 

/*  GLOBALS_H  */ 
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/*  ^ 
Program:  globals .c 

Authors:  Don  Bmtzman,  Duane  Davis 


Revised: 


2  Agust  96 


System: 

Compiler: 

Compilation: 

[68020] 

[68030] 

[Irix  ] 

Purpose : 


AUV  Gespac  68020/68030,  OS-9  version  2.4 
Gespac  cc  Kernighan  &  Richie  (K&R)  C 

ftp>  put  globals.c 
auvsiml>  chd  execution 
auvsiml>  make  -k2f  execution 
auvsiml>  make  execution 

fletch>  make  execution 

Allows  repeated  use  of  global  variables  global. c  via  global. h 
in  order  to  prevent  compiler  warnings 

See  statevector .c/statevector .h  for  other  global  variables 


*/ 


#ifndef  GLOBALS^C  ^ 

#define  GLOBALS_C 

#include  "globals.h" 


/*  Program  configuration  flags  */ 


int 

TRACE 

=  0; 

/* 

l=trace  on,  0=trace  off 

V 

int 

DISPLAYSCREEN 

=  1; 

/* 

l=screen  on,  0=screen  off 

*/ 

int 

LOCATIONLAB 

=  1; 

/* 

l=virtual  world, 0=actual  vehicle 

*/ 

int 

BENCHTEST 

-  0; 

/* 

l=virtual  world, 0=actual  vehicle 

*/ 

int 

TACTICAL 

=  0; 

/* 

l=tactical  on,  0=tactical  off 

*/ 

int 

LOOPFOREVER 

=  0; 

/* 

l=:repeat  execution  indefinitely 

*/ 

int 

LOOPFILEBACKUP 

==  1; 

/* 

l=backup  files  between  replications*/ 

int 

PARALLELPORTTRACE 

=  0; 

/* 

l=:trace  each  char  received  at  port 

*/ 

int 

SONAR INSTALLED 

=  1; 

/* 

l=:sonar  head  available  for  query 

*/ 

int 

SONARTRACE 

=  0; 

/* 

l=trace  on,  0=trace  off 

*/ 

int 

SONARSCANMODE 

=  1; 

/* 

l=forward,  2=target  tracking 

*/ 

double 

SONARHEADINGSTEP=  0.9; 

/*  degrees 

*/ 

’int 

SONARSCANDIRECTION 

=  1; 

/* 

l  =  right,  2  =  left 

*/ 

double 

stl000_command 

o 

o 

■  II 

/* 

degrees  commanded  bearing 

*/ 

int 

ENTERCONTROLCONSTANTS 

=  0; 

/* 

1  =  manual  entry,  0=default  values 

i*/ 

int 

SHOWCONTROLCONSTANTS 

=  0; 

/* 

1  =  print  constants,  0=default 

*/ 

int 

LOADCONTROLCONSTANTS 

=  1; 

/* 

1  =  file  entry,  0=default  values 

*/ 

int 

REALTIME 

=  0; 

/* 

1  =  real-time  waits,  0  =  no-pause 

*/ 

int 

DIVETRACKER 

=  0; 

/* 

l=dive  tracker  being  used 

*/ 

int 

DEADRECKON 

=  0; 

/* 

l=dead  reckon  navigate,  0=regular 

*/ 

int 

DEADSTICKRUDDER 

=  0; 

/* 

l=use  ordered  rudder,  0  =  control 

*/ 

int 

DEADSTICKPLANES 

=  0; 

/* 

l=use  ordered  planes,  0  =  control 

*/ 

int 

INTEGRALDE  PTHCONTROL 

=  0; 

/* 

l=:use  PID,  0  =  use  PD 

*/ 

double 

time_int_control_on 

=  1000000 

.0;  /*  give  PD  a  chance  to  get 

close*/ 

/*  untested  */ 

int 

SLIDINGMODECOURSE 

=  0; 

/* 

l=use  sliding  mode,  0  =  control 

*/ 
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int 

THRUSTERCONTROL 

=  0; 

/* 

l=:usG  thrusters,  0=use  propellers 

*/ 

int 

ROTATECONTROL 

=  0; 

/* 

l=use  thrusters  to  rotate  in  place 

*/ 

int 

LATERALCONTROL 

=  0; 

/* 

l=use  thrusters  for  lateral  motion 

*/ 

int 

FOLLOWWAYPOINTMODE 

=  0; 

/* 

1=  go  to  WAYPOINT  without  WAITS 

*/ 

int 

WAYPOINTCONTROL 

=  0; 

/* 

1=  go  to  WAYPOINT 

*/ 

int 

HOVERCONTROL 

=  0; 

/* 

l=hover  at  WAYPOINT 

*/ 

int 

TARGETCONTROL 

=  0; 

/* 

l=hover  relative  to  a  target 

*/ 

int 

NEWTARGET 

=  0; 

/* 

l=target  is  new,  0=target  is  old 

*/ 

int 

TARGETPOINTING 

=  0; 

/* 

l=point  at  target  if  TARGETCONTROL 

*/ 

int 

TARGETEDGETRACK 

=  0; 

/* 

l=tracking  on  target  edge  only 

*/ 

int 

NEWTARGETSTATION 

=  0; 

/* 

l=new  station  keeping  point 

*/ 

int 

RECOVERYCONTROL 

=  0; 

/* 

l=recovery  in  progress 

*/ 

int 

NEWRECOVERYCOMMAND 

=  1; 

/* 

l=command  new,  0=in  progress 

*/ 

double 

SCANWIDTH 

=  30.0 

;/* 

degrees  full  scan,  centered  on  bow 

*/ 

/* 

90  degrees  takes  --15  seconds 

/* 

45  degrees  probably  optimal 

*/ 

int 

LEAK 

=  0; 

/* 

l=water  leak  in  progress 

*/ 

int 

HALTSCRIPT 

=  0; 

/* 

l=automatic  shutdown  criteria  met 

*/ 

int 

DEATH_S  P  IRAL_RESET 

=  1; 

/* 

l=reset  death  spiral  checker 

*/ 

#ifndef  os9 

int 

#else 

EMAIL 

=  0; 

/* 

l=send  e-mail,  0=don't  send  e-mail 

*/ 

int 

#endif 

EMAIL 

=  0; 

/* 

can't  send  email  via  OS-9  directly 

*/ 

int 

EMAIL^ENTERED 

=  0; 

/* 

flag  for  first  time  through 

V 

int 

*/ 

int 

NOT_YET_REIMPLEMENTED 

=  0; 

r 

*  code  in  block  needs  reverification 

ARCHAIC^IGNORE 

=  0; 

/* 

code  in  block  not  valid,  commented 

*/ 

double 

TIMESTEP 

=  0.15; 

/* 

time  of  a  single  closed  loop 

*/ 

/* 

add  code  to  warn  if  exceeded  «« 

*/ 

/* 

units  are  seconds 

*/ 

int 

TACTICALPARSE 

=  0; 

/* 

l=tactical  level  parsing  commands 

*/ 

int 

KEYBOARDINPUT 

=  0; 

/* 

l=read  keyboard  vice  mission  file 

*/ 

int 

HELPFILELAUNCHED 

=  0; 

/■ 

*  l=mission. script .HELP  already 

shown*, 

/ 

int 

GPSFIXINPROGRESS 

=  0; 

/* 

l=wait  GPS-FIX  &  restore  z_command 

*/ 

int 

REPORTSTABLE 

=  0; 

/* 

l=tell  when  stable  hover /way pt/gps 

int  REPLAY  =0;  /*  l=rGplay  of  existing  telemetiry  file  */ 

int  NOSCRIPT  =0;  /*  l=replay  of  existing  telemetry  file  */ 

/*  with  no  accompanying  script  file  */ 

/*******************★****************★**************************************/ 
/*  files  and  paths  */ 

FILE  *  auvscriptf ile; 

FILE  *  auvordersf ile; 

FILE  *  auvdatafile; 

FILE  *  auvtextfile; 

FILE  *  telemetry_file; 

FILE  *  controlconstantsinputf ile; 

FILE  *  controlconstantsoutputfile; 

FILE  *  emailaddressf ile; 

FILE  *  stlOOOdatafile; 

FILE  *  targetdatafile; 

/*  FILE  *  serialtestfile;  */ 

char  TELEMETRYFILENAME  [255];  /*  located  here  due  to  gespac  cross-compiler  */ 


int  serialpath  =  0; 


int  sonarpath  =  0; 

/***★************************** ***************************************^^^^^^^ 
/*  Variables  and  data  structures  */ 

/*  buffers  of  full  strings  for  byte  transfer  to  tactical  level  &  disk  file  */ 
/*  "buffer'  usually  <  256,  intentionally  oversized  in  case  of  overflow  error*/ 

tiine_t  system^time  =  0; 

struct  tm  *systeiTi_tinp  =  0; 


/*  dac :  digital-analog  converter  */ 

/*  adc :  analog-digital  converter  */ 

/*  4  Channels  of  DAC  ADA-1  DAC  --  updated  */ 

unsigned  char  *dacl_a  =  (unsigned  char  *)  DAC1_ADDR; 

/*  8  Channels  of  DAC  DAC-2B  --  updated  */ 

unsigned  char  *dac2b_a  =  (unsigned  char  *)  DAC2B_ADDR; 

/*  16  Channels  of  ADC  ADA-1  —  updated  */ 

unsigned  char  *adcl_a  =  (unsigned  char  *)  ADC1_ADDR; 

/*  16  Channels  of  ADC  ADC-2  —  updated  */ 

unsigned  short  *adc2_a  =  (unsigned  short  *)  ADC2_ADDR; 


unsigned  char  *via0  =  (unsigned  char  *)  VIA0_ADDR; 
unsigned  char  *vial  =  (unsigned  char  *)  VIA1_ADDR; 

unsigned  char  viaOa^reg,  via0b_reg; 


int 

teleinetry__records__saved  =  0; 

int 

mission_leg_counter 

=  0; 

int 

r ep 1 i c a t i an_c ou n t 

=  1; 

int 

end_test 

=  FALSE; 

int 

wrap_count 

=  0; 

double 

dt 

=  0.15;  /*  units  are  seconds  */ 

double 

rpm 

=  0.0;  /*  +-700  rpm  ==  +-2  ft/sec  */ 
/*  (steady  state)  */ 

double 

dt^time 

;  /*  dive  tracker  time  */ 

double 

computer^voltage 

=  24.0; 

double 

inotor_voltage 

=  24.0; 

double 

vertical_thruster_volts 

=  0.0;  /*intermediate  calculation  */ 

double 

lateral_thruster_volts 

=  0.0;  /*intennediate  calculation  */ 

double 

■  inain_inotor_deltal 

=  0.0; 

double 

main_inot  or_del  ta2 

=  0.0; 

int 

ma i n_mo t  o r_vo 1 1 1 

=  512; 

int 

main_inotor_yol  t2 

=  512; 

unsigned  short  psi_bit_old 

=  0; 

double 

dg_off set 

=  0.0; 

double 

startups!  = 

=  0.0;  /*initial  heading  in  degrees  */ 

/*  Used 

to  estimate  the  X  and  Y  position  of  the  AUV  */ 

double 

X  est 

=  0.0; 

double 

Y_est 

=  0.0; 

double 

X_dot_est 

=  0.0; 

double 

Y_dot_est 

=  0.0; 
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double 

double 


u_est 
V  est 


=  0.0; 
=  0.0; 


/*control  coefficients  are  based  on  standard  units  (degrees/feet/seconds) */ 


double 

k_psi 

=  0.0; 

double 

k_r 

=  0.0; 

double 

k_v 

=  0.0; 

double 

k_2 

=  0.0; 

double 

k_w 

=  0.0; 

double 

k  theta 

=  0.0; 

double 

k^g 

=  0.0; 

double 

k_thruster_psi 

=  0.0; 

double 

k_thruster_r 

=  0.0; 

double 

k_thruster_rotate 

=  0.0; 

double 

k_th2ruster_l  ateral 

=  0.0; 

double 

k_thruster_z 

=  0.0; 

double 

k_thruster_w 

=  0.0; 

double 

k_t  h  m  s  t  e  r_t  he  t  a 

=  0.0; 

double 

k_propeller_hover 

ti 

o 

o 

double 

k_surge_hover 

II 

o 

o 

double 

k_propeller_current 

II 

o 

o 

double 

k_thruster_hover 

=  0.0; 

double 

k_sway_hover 

=  0.0; 

double 

k_thruster__current 

=  0.0; 

double 

k_sigma_r 

=  12.0. 

double 

k_sigma_psi 

=  28.87; 

double 

eta_stGering 

=  0.1. 

double 

sigma 

=  0.0. 

int 

mission_legs_total 

=  0; 

/* 

values  initialized  in  parse_mission_script  commands  ()  */ 

double 

t  ime_next_command 

=  0.0; 

/* 

units  are  seconds 

double 

time_gps_complete 

=  0.0; 

/* 

units  are  seconds 

double 

t  ime_pos  tgps_di  ve 

=  0.0; 

/* 

units  are  seconds 

double 

psi_command 

=  0.0; 

/* 

degrees  */ 

double 

p s i_c ommand_ho ve r 

=  0.0; 

/* 

degrees  */ 

double 

theta_command 

=  0.0; 

/* 

degrees  */ 

double 

x_command 

=  0.0; 

/* 

feet  */ 

double 

y_command 

=  0.0; 

/* 

feet  */ 

double 

z_command 

=  0.0; 

/* 

feet  */ 

double 

s  tbd_rpm_command 

=  0.0; 

/* 

-700. .700  */ 

double 

port_rpm_command 

=  0.0; 

/* 

-700. .700  */ 

double 

planes^command 

=  0.0; 

/* 

degrees  */ 

double 

rudder_command 

=  0.0; 

/* 

degrees  */ 

double 

rotate_command 

=  0.0; 

/* 

degrees /sec  */ 

double 

lateral_command 

=  0.0; 

/* 

ft/sec  */ 

double 

bow_lateral_thruster_command 

=  0.0; 

/* 

volts  -24. .24  */ 

double 

stern_lateral_thruster_command 

=  0.0; 

/* 

volts  -24. .24  */ 

double 

bow__ve  r  t  i  c  a  1  _t  hr  u  s  t  e  r_comma  nd 

=  0.0; 

/■^ 

volts  -24. .24  */ 

double 

s  t  er  n__ve  r  t  i  c  a  1  _  t  h  r  u  s  t  e  r_c  omma  nd 

=  0.0; 

/* 

volts  -24. .24  */ 

double 

previous_z_command 

=  0.0; 

/* 

feet  */ 

double 

range_f  r  om_r  ec  overy_p  t 

=  0.0; 

/* 

feet  */ 

double 

gyro_error 

=  0.0; 

/* 

degrees  */ 

double 

waypoint_di stance 

=  0.0; 

/* 

feet  */ 

double 

waypoint_angle 

=  0.0; 

/* 

degrees  */ 

double 

track_angle 

=  0.0; 

/* 

degrees  */ 

double 

along_track_di stance 

=  0.0; 

/* 

feet  */ 

*/ 

*/ 

*/ 
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double 

cross__track_di  stance 

0.0; 

/* 

feet 

*/ 

double 

standof  f__distance 

= 

2.5; 

/* 

feet 

*/ 

double 

death__spiral_radius 

= 

15.0 

;  / 

*  feet 

*/  . 

double 

depth_error 

/* 

feet 

*/ 

double 

depth_cell_bias 

= 

o.oi 

/* 

feet 

V 

double 

psi_error 

= 

0.0; 

/* 

degrees 

*/ 

/*  parameters 

required  for  targetcontrol 

*/ 

double 

target_x 

= 

0.0; 

/* 

feet,  world 

coords 

*/ 

double 

target_y 

= 

0.0; 

/* 

feet,  world 

coords 

*/ 

double 

target_z 

= 

0.0; 

/* 

feet,  world 

coords 

V 

double 

target_bearing 

= 

0.0; 

/* 

degrees 

*/ 

double 

ta  rge  t__bea  r  i  ng_c  ommand 

= 

0.0; 

/* 

degrees 

*/ 

double 

ta  rge  t_bea  r  i  ng__do  t 

0.0; 

/* 

degrees  per 

second 

V 

double 

target_range 

= 

0.0; 

/* 

feet 

*/ 

double 

target_range_coinmand 

= 

0.0; 

/* 

feet 

*/ 

double 

targe  t_r  ange__do  t 

= 

0.0; 

/* 

feet  per  second 

V 

/*  psi  i~l  for 

differentiation  of  r  needed 

because 

of  busted  gyro  */ 

double 

psi_iml 

0.0; 

/* 

degrees 

*/ 

/*  values  used  by  kahlman  depth  filter  */ 
int 

double 
double 
double 
double 

int 
int 
int 
int 
int 
int 
int 
int 
int 
int 
int 
int 

double 
double 
double 
double 
double 
int 
int 

int 

int 

int 
int 
int 
int 
int 

int 
long 
long 
double 
short 
char 
int 


kal„init__z 

= 

TRUE; 

thres_z 

= 

1.0; 

z_kal 

0.0; 

z_dot_kal 

= 

0,0; 

z_ddot_kal 

= 

0.0; 

roll__rate_0 

= 

0; 

pitch_rate_0 

0; 

yaw_rate  0 

= 

0; 

roll_0 

0; 

pitch_0 

= 

0; 

z__val0 

= 

0; 

swl 

= 

0; 

error 

= 

0; 

range 

0; 

bad_rng 

= 

0; 

bad_updates 

= 

0; 

range_index 

= 

0; 

rangel 

= 

0.0; 

range2 

= 

0.0; 

errorl 

= 

0.0; 

error2 

= 

0.0; 

avg_rng 

=: 

0.0; 

k__range 

rang e_ar ray  [3000] ; 

0; 

speed_array  [11]; 

PortAFlag 

= 

0; 

tick 

_ 

0; 

curr_tick 

= 

0; 

tickl 

0; 

tick2 

0; 

i 

= 

0; 

mask 

= 

OxOOOOffff ; 

davedate 

0; 

da ve time 

= 

0; 

value 

— 

0.0; 

day 

0; 

answer 

/ 

start__dwell 

= 

0; 
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int 

socket_descriptor 

0; 

int 

socket_accepted  = 

0; 

int 

socket_stream  = 

0; 

int 

tactical_socket_descriptor  =  0; 

int 

tactical__socket_accepted 

=  0; 

int 

tactical_socket_stream 

=  0; 

int 

socketjength  = 

MAXBUFFERSIZE 

int 

buffer_jnax  = 

MAXBUFFERSIZE 

/*  char  buffer_array  [FILEBUFFERSIZE]  [256];  --  not  implemented  */ 


char  buffer  [MAXBUFFERSIZE  +  10]; 

char  local_buffer  [MAXBUFFERSIZE  +  10]; 


int  buffer_size  =  0; 
int  buffer_index  =  0; 
int  variables  ^parsed  =  0; 


char 


int 

int 

int 

int 

int 

FILE 


buffer__received  [MAXBUFFERSIZE  +  10]  , 

virtual_world_remote_host__name  [60]  , 
tactical_remote_host_name  [60], 
command_buffer  [MAXBUFFERSIZE  +  10] ; 


bytes_received  =  0; 
t>ytes_read  =  0; 
bytes_written  =  0; 
bytes_left  =  0; 
bytes_sent  =  0; 


*  netstat_fileptr; 


struct  sockaddr_in  server_address; 


struct  hostent  *server__entity; 


char 

int 

int 

int 

char 

int 

double 


email_address  [81]  ; 

shutdown__signal_receiv€d  =  FALSE; 
virtual_world_socket_opened  =  FALSE; 
tactical_socket_opened  =  FALSE; 


*  ptr_index; 
print_help 


=  FALSE; 


speed_per_rpm  =  2.0  /  700.0  ;  /*  steady  state: 

2.0  feet /sec  per  700  rpm  */ 
/*  -700. .700  */ 


docket 

docket 

int 

int 


nextloopdock  =  0; 

currentloopdock  =  0; 

audible_command  =  TRUE; 


auvscriptfilequit  =  FALSE; 


double  AUV_oceancurrent_x 
double  AUV_oceancurrent_y 
double  AUV_oceancurrent_z 


=  0.0; 

/* 

Ocean 

current 

=  0.0; 

/* 

Ocean 

current 

=  0.0; 

/* 

Ocean 

current 

rate  along  North-axis  */ 
rate  along  East-axis  */ 
rate  along  Depth-axis  */ 


double  DiveTrackerl_x; 
double  DiveTrackerl_y  ; 
double  DiveTrackerl_z; 


/*  DiveTrackerl 
/*  DiveTrackerl 
/*  DiveTrackerl 


transducer  x  (feet) 
transducer  y  (feet) 
transducer  z  (feet) 


*/ 

*/ 

*/ 


double  DiveTracker2_x; 
double  DiveTracker2_^; 


/*  DiveTracker2  transducer  x  (feet) 
/*  DiveTracker2  transducer  y  (feet) 


*/ 

*/ 
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double  DiveTracker2_z; 


/ 


/*  DiveTracker2  transducer  z  (feet) 


/*  Dave's  cats  and  dogs  */ 

unsigned  char  *tim_lacl  =  (unsigned  char  *)  TIM_1AC_1; 

unsigned  char  *tim__lac2  =  (unsigned  char  *)  TIM_1AC_2; 

unsigned  char  *tim_lac3  =  (unsigned  char  *)  TIM_1AC_3; 

unsigned  char  tiin_la__data_reg  =  TIM_1AC_DATA_REG; 

unsigned  char  tim_la_control_reg  =  TIM_1AC_C0NTR0L_REG; 
unsigned  char  tiin_la_aux_gates_reg  =  TIM_1AC_AUX_GATES_REG; 

tifdef  os9 

char  *dt_fork_panTiptr;  • 
char  *argblk[]  =  { 

"unlink" , 

"DT2CL", 

0, 

}; 

int  dtjpid; 
int  ul_pid; 

#endif 

/**********************************★**************************************** ^ 
tendif 

/*  GLOBAL S_C  */ 
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/*  ^ 
Prog  ram :  ext  e  rna  l_f  unc  t  i  ons .  c 

Authors:  Don  Brutzman 


Revised: 


2  August  96 


System:  AUV  Gespac  68020/68030,  OS-9  version  2.4 

Compiler:  Gespac  cc  Kernighan  &  Richie  (K&R)  C 


Compilation:  ftp>  put  external_functions .c 

auvsiml>  chd  execution 

[68020]  auvsiml>  make  -k2f  execution 

[68030]  auvsiml>  make  execution 


[Irix  ] 
Purpose: 


fletch>  make  execution 

Reduce  size  of  execution. c  to  allow  OS-9  C  compiler  to  work 

Make  functions  globally  available  for  tactical  level 
in  order  to  guarantee  compatibility 


Note  that  %F  double  formats  are  used  instead  of  %lf  on  scanf()  and  sscanf()‘ 
calls  for  OS-9  compatibility.  SGI  C  compiler  does  not  complain, 
gcc  on  Sun  does  fail  at  run-time,  so  ifdef's  are  used  to  support 
the  proper  format  string. 


*/ 


/************★*************★******************************* *****************^ 
/*  external_f unctions .c  */ 

#include  "globals.h" 

#include  "statevector .h" 

#include  "defines. h" 

/*************************************************************************** ^ 
/*  OS-9  -  specific  function  compatibility  */ 

/*  (mostly  stubs  to  permit  compilation  under  Unix)  */ 

#ifndef  os9 

void  tsleep  (unsigned  svalue)  {  /*  null  body  */} 

void  _sysdate  (format,  time,  date,  day,  tick) 

int  format,  *time,  *date,  *tick;  short  *day; 

{  /*  null  body  */} 

double  pow  (xx,  yy) 

double  XX,  yy; 

{return  exp  (yy  *  log  (xx) ) ; } 


int  _gs_rdy  (path)  int  path;  {  return  0;  }  /*  bytes  waiting  on  path  */ 

int  _gs__opt  (path,  buffer) 

int  path;  struct  sgbuf  *buffer; 

{  return  0;  } 


int  _ss_opt  (path,  buffer) 

int  path;  struct  sgbuf  *buffer; 
{  return  0;  } 

#endif 


double  degrees 


{)  ; 
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double 

radians 

0  ; 

double 

normalize 

0  ; 
{)  ; 

double 

normalize2 

double 

radian_normalize 

0  ; 

double 

radian_normal i z  e2 

0  ; 

void 

clamp 

0  ; 

double 

atan2z 

0  ; 

#ifdef  os9 

double 

atan2 

0  ; 

double 

sinh 

0  ; 

double 

cosh 

{) ; 

double 

tanh 

0  ; 

#endif 

void 

build_telemetry_string 

0  ; 

void 

parse_telemetry_string 

0; 

void 

r  e  ad__  t  e  1  erne  t  ry  __s  t  r  i  ng 

0; 

void 

open_tactical_socket 

0; 

void 

shutdown_tactical_socket 

0; 

void 

send__bu  f  f  er_t  o_t  ac  t  i  ca  l_s  oc  ke  t 

0  ; 

void 

get_string_from__tactical_socket 

0; 

void 

record_data_on 

0; 

void 

record_da  ta_o  f  f 

0; 

void 

cage_dg 

0 

void 

uncage_dg 

0  ; 

int 

detect_death_spiral 

0  ; 

double 

dsign 

0  ; 

double 

dtanh 

{) ; 

/★******★*★****************************************★************************/ 

double  degrees  (rads)  /*  radians  input  */ 

double  rads; 

{ 

return  rads  *  180.0  /  PI; 

} 

/************★**************************************************************/ 

double  radians  (degs)  /*  degrees  input*/ 

double  degs; 

{ 

return  degs  *  PI  /  180,0; 

} 

/******************★************************************************★*******/ 

double  normalize  (degs)  /*  degrees  input*/ 

double  degs; 

{ 

double  result  =  degs; 

while  (result  <  0.0)  result  +=  360.0; 
while  (result  >=  360.0)  result  -=  360.0; 

return  result; 

} 

/*************★*************************★★*************************★********/ 

/*  degrees  input*/ 


double  normal ize2  (degs) 
double  degs; 

{ 
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double  result  =  degs; 


while  {result  <=  -180.0)  result  +=  360.0; 
while  (result  >  180.0)  result  -=  360.0; 

return  result; 

} 

/***************************★*★****************************************  j^.****^ 

double  radian_nonnalize  (rads)  /*  radians  input*/ 

double  rads; 

{ 

double  result  =  rads; 

while  (result  <  0.0)  result  +=  2.0  *  PI; 

while  (result  >=  2,0  *  PI)  result  -=  2.0  *  PI; 

return  result; 

} 

/******************************★******************************************** ^ 

double  radian_normalize2  (rads)  /*  radians  input*/ 
double  rads; 

{ 

double  result  =  rads; 

while  (result  <=  -  PI)  result  +=  2.0  *  PI; 

while  (result  >  PI)  result  -=  2.0  *  PI; 

return  result; 

} 

/**********************★**************************************************** ^ 

void  clamp  (clampee,  absolute_min,  absolutejnaax,  name) 
double  *  clampee; 
double  absolute_jnin; 
double  absolute^max; 
char  *  name; 

{ 

double  new_value,  local_min,  local_jnax; 

if  ( (absolute_max  ==  0.0)  &&  (absolute_min  ==  0.0))  return;  /*  no  clamp  */ 

if  (absolute_max  >=  absolute_min)  /*ensure  min  &  max  used  in  proper  order  */ 

local__min  =  absolute_min; 
local_max  =  absolute_max; 

} 

else 

{ 

local_min  =  absolute_max; 
local_max  =  abso lu terrain ; 

} 

if  ((*  clampee)  >  local_max) 

{ 

new_value  =  local_max; 

if  (TRACE  ScSc  DISPLAYSCREEN) 

printf  ("[clamping  %s  from  %5.3f  to  %5.3f]\n", 
name,  *  clampee,  new_value) ; 

*  clampee  =  new_value; 

} 

if  ((*  clampee)  <  local_min) 

{ 

new_value  =  local_min; 
if  (TRACE  &&  DISPLAYSCREEN) 
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printf  ("[clamping  %s  from  %5.3f  to  %5.3fl\n", 
name,  *  clampee,  new_value) ; 

*  clampee  =  new_value; 

} 

} 

/*********★**********★***********★******************************************/ 

double  atan2z  (x,  y) 

double  y;  double  x; 

{  /*  more  reliable  (0, 0 ) -protected  atan2  due  to  cross-platform  noncompliance 
*/ 


if  {(fabs  (X)  ==  0.0)  &&  (fabs  (y)  ==  0.0))  return  0.0; 

else  return  atan2  (x,  y)  ; 

} 

/************** *************************************************************^ 
#ifdef  os9 


/*  thanks  to  Michael  Olberg  Oct  20,  94  olberg@bele.oso.chalmers.se 
/*  modified  to  map  (0,0)  to  0.0 


double  atan2 

{ 


(Y/  X) 

double  y; 


double  X; 


if  (TRACE  &&  DISPLAYSCREEN) 

printf  ("[atan2  (%5.3f. 


%5.3f) ]\n",  y,  x) ; 


if  (fabs  (x)  ==  0.0)  { 

if  (y  <  0.0)  return(-PI/2.0) ; 

else  if  (fabs  (y)  ==  0.0)  return  0.0; 
else  return (  PI/2.0); 

}  else  { 

if  (x  <  0 . 0)  { 

if  (y  <  0.0)  return (atan (y/x) -PI)  ; 
else  return (atan (y/x) +PI) ; 

}  else  return (atan (y/x) ) ; 

} 

} 

/*  as  to  the  tanh  you  will  simply  have  to  use  */ 


double  sinh  (x) 
double  X; 

{ 

return  (exp(x)  -  exp (-x) ) /2 .0; 

} 

double  cosh  (x) 
double  X; 

{ 

return  (exp(x)  +  exp (-x) ) /2 .0; 

} 

double  tanh  (x) 
double  X; 

{ 

return  sinh (x) /cosh (x) ; 

} 


#endif 


*/ 

*/ 


/***************************************************************************/ 
void  build_telemetry_string  ( telemetry_buf f erjtr) 
char  *  telemetry _buffer_ptr; 
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{  /*  this  function  parses  global  string  buf fer_received  in  execution. c  */ 

if  (TRACE  &&  DISPLAYSCREEN)  printf  ("[begin  build_teleinetry_string  {)]\n"); 

for  (i  =  0;  i  <=  buffer_max;  i++)  buffer  [i]  =  '\0';  /*  zero  buffer  */ 

/*  auv_state  means  state  vector  from  auv  that  is  going  to  virtual  world  */ 

if  (LOCATIONLAB)  z  =  2  -  depth_cell_bias ; 

buffer_size  =  sprintf  ( telemetry _buffer_ptr, 

"  auv__state  %5.3f  %5.3f  %5.3f  %5.3f  %5.3f  %5.3f  %5.3f  %5.3f  %5.3f  %5.3f  %5.3f 
%5.3f  %5.3f  %5.3f  %5.3f  %5.3f  %5.3f  %5.3f  %5.3f  %5.3f  %5.3f  %5.3f  %5.3f  %5.3f 
%5.3f  %5.3f  %5.3f  %5.3f  %5.3f  %5.3f  %5.3f  %5.3f  %5.3f  %5.3f  %5.3f  %5.3f  \n" . 

t, x,y, z, 

normalize2  (phi  ) , 
normalize2  (theta) , 
normalize  (psi  ) , 
speed, 

u,  V,  w, 
normalize2  (p) , 
normalize2  (q) , 
normal ize2  (r) , 
x_do  t ,  y_do t ,  z_do  t , 
normalize2  (phi_dot  ), 
normal ize2  (theta_dot) , 
normal ize2  (psi_dot  ) , 
normali2e2  (delta_rudder) , 
normalize^  (delta_planes) , 

po  r  t_rpm ,  s  tbd_rpm , 

AUV_bow_ve  r  t  i  c  a  1 ,  AUy_s  t  ern_ve  rt  i  c  al , 

AUV_bow__lateral ,  AUV_stern_lateral , 

AUV_ST1000_bearing,  AUV_ST1000_range,  AUV_ST1000_strength, 

AUV_ST7  2  5_bear  ing ,  AUV_ST7  2  Strange ,  AUV_ST7  2  5_s  t  reng  th , 

divetracker_rangel ,  divetracker_range2 ) ; 

if  (LOCATIONLAB)  z  =  z  +  depth_cell_bias; 

if  (buffer^size  >  buffer_jnax)  /*  sprintf  buffer  overflow  condition  */ 

{ 

if  (DISPLAYSCREEN) 

printf  ("Buffer  overflow,  buffer_size  =  %d,  reduced  to  %d  ! ‘ ! ! i ‘ \n" , 

buffer^size,  buf  fer__max)  ; 
buffer_size  =  buffer_max; 


if  (TRACE  &&  DISPLAYSCREEN)  printf  ( " [buf f er^size  is  %d]\n",  buf fer^size) ; 


*  /*  other  state  variables  &  timing  constraints  can  be  added  «<««««««  */ 

if  (TRACE  &&  DISPLAYSCREEN)  printf  ("[finish  build_telemetry_string 
()]\n"); 

return; 

) 

/***********************************★*******************★*******************/ 
void  parse_telemet]:y_string  (passed_buf fer_ptr) 
char  *  passed_buf fer_ptr; 

{  /*  this  function  parses  global  string  buf fer_received  in  execution. c  */ 

/*  temporary  hold  variables  */ 
double  AUV_time_temp, 

AUV_x_temp,  AUV^_temp,  AUV_z_temp, 
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AUV_phi_teTnp , 

AUV_speed_teTnp , 

AUV_u_t:emp/ 

AUV_p_teinp/ 

AUV_x_do  t_t  emp , 

AUV_ph  i_do  t_t  emp , 

AUV_p  s  i  _do  t_t  emp , 

AUV__del  ta__rudder_temp , 
AUV_por  t_rpm_temp , 
AUV__bow_vert  i  cal_temp , 
AUV_bow__l  a  t  er  al_t  emp , 

AUV_ST1 000_bearing_temp , 
AUV_ST1 0  0  0_s  t  rength_t  emp , 
AUV_ST7  2  5_range_t  emp , 
AUV_divetracker_rangel_temp, 


AUV_theta_temp , 


AUVjpsi_temp, 


AUV__v_temp , 
AUV_q_temp , 

AUV _y_dot_temp, 
AUV_the  t  a_do  t_temp , 


AUV_w_temp, 
AUV_r_Cemp, 
AUV_z_dot_t emp , 


AUV_de 1 t a_pl anes_t emp , 

AUV_s  t  bd_rpm_t  emp , 

AUy_s  t  er  n_ve  r  t  i  c  a  l_t  emp , 
AUV_s  t  er  n__l  a  t  e  r  a  l_t  emp , 
AUV_ST1 0  0  0_r ange_t  emp , 
AUV_ST7  2  5_bear ing_temp , 
AUV_ST7  2  5_s  t  r  eng  t  h_t  emp , 
AUV_di  vetracker_range2_temp  ; 


if  (TRACE  &&  DISPLAYSCREEN)  printf  {"  [begin  parse_telemet2:y_string  ()]\n"); 

/*  update  and  output  AUV  state  variables  */ 

/*  note  that  if  dead  reckoning  is  used,  values  will  not  change  */ 

/*  note  %F  required  by  OS-9,  accepted  by  SGI  as  equivalent  to  %lf  */ 

if  (TRACE  ScSc  DISPLAYSCREEN) 

printf  ("from  telemetry  buffer : \n%s" ,  passed__buffer_ptr) ; 


variables_parsed  =  sscanf (passed_buf f er_ptr, 
iifndef  os9 

"%s  %lf  %lf  %lf  %lf  %lf  %lf  %if  %if  %if  %if  %if  %if  %if  %if  %if  %if 
%lf  %lf  %lf  %lf  %lf%lf  %lf  %lf  %lf  %lf  %if  %if  %if  %if  %if  %if  %if  %if  %if 
%lf \n" , 

#else 

"%s  %F  %F  %F  %F  %F  %F  %F  %F  %F  %F  %F  %F  %F  %F  %F  %F  %F  %F  %F  %F  %F  %F 
%F  %F  %F  %F  %F  %F  %F  %F  %F  %F  %F  %F  %F  %F\n", 

#endi f 


keyword, 

5cAUV_x_temp , 

&AUV_phi_temp , 
&AUV_speed_t  emp , 
&AUV_u_temp , 

&AUV_p__temp, 

ScAUV_x_do  t_t  emp , 
&:AUV_phi„dot_temp , 

&AUy_del  ta_rudder__temp , 
ScAUV_port__rpm_temp , 
&:AUV_bow_verti  cal_temp , 
&AUV_bow_l  a  t  er  al_t  emp , 
&AUV_ST1 0  0  0_bear  i  ng_t  emp 

&AUV_ST725_bearing_temp , 

£cAUV_divetracker_rangel_l 


&AUV_t  ime_t  emp , 
&:AUV_y_temp , 
&AUV_theta_temp , 


5cAUV_z_temp , 

&AUV_p  s  i_t  emp , 


&AUV_v_t  emp ,  ScAUV__w_t  emp , 

ScAUV_q__t  emp ,  &AUV_r_t  emp , 

ScAUV_y_do  t  _t  emp ,  &  AUV_z_do  t_t  emp , 

&AUV_theta_dot_temp,  &AUV_psi_dot_temp, 
&AUV_del  ta_j)lanes_temp , 

ScAUV__s  tbd_rpm_temp , 

&AUV_s  t  ern_ver  t  i  ca  l_t  emp , 

&AUV_s  tern_l  at  eral_temp , 

&AUV_ST1 0  0  0_range_temp , 

&AUV_ST1 0  0  0_s  t  rength^t  emp , 

&AUV_ST7  2  5„r  ange_t  emp , 

ScAUV_ST7  2  5_s  t  r  engt  h_temp , 
temp,  &AUV„divetracker_range2_temp)  ; 


if 

{ 


(variables_parsed  ==  STATEVECTORSIZE)  /*  transfer  OK,  keep  new  values  */ 


t 

X 

y 

z 

phi 

theta 

psi 

speed 

u 

V 

w 

P 

q 


(AUV_time_temp)  ; 
(AUV_x_temp)  ; 
(AUV_y_temp)  ; 
{AUV_z_l:emp)  ; 
(AUV_jphi_temp)  ; 
(AUV_theta_temp) ; 
(AUV_psi_temp) ; 
{AUV_speed__temp)  ; 
(AUV_u_temp)  ; 
(AUV_v_temp)  ; 
(AUV_w„temp)  ; 
(AUV__p_temp)  ; 
(AUV_q_temp)  ; 
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} 


r 

x_dot 

y_dot 

z_dot 

phi_dot 

theta_dot 

psi_dot 

delta_rudder 

delta_planes 

port_rpm 

stbd^rpm 

AUV__bow_v  e  r  t:  i  c  a  1 
AUV_s t e r n_v e r t i c a 1 
AUV_bow_l a  t  er al 
AUV_s  tern_lateral 
AUV_ST1000_bearing 
AUV_ST1 00  Derange 
AUV_ST1 00  0_st rength 
AUV_ST725_bearing 
AUV_ST725_range 
AUV_ST7  2  5_s  t rength 
divetracker_rangel 
divetracker_range2 


=  (AUV_r_teinp)  ; 

=  (AUV_x_dot_tenip)  ; 

=  ( AUV _7_dot_temp )  ; 

=  (AUV_z_dot_teinp) ; 

=  (AUV_phi_dot_teinp) ; 

=  {AUV_theta_dot_teinp)  ; 

=  ( AUV_p  s  i_do  t_  t  emp ) ; 

=  (AUV_delta_rudder_teTnp)  ; 

=  (AUV_delta_planes_teinp)  ; 

=  (AUV_port_rpm_temp)  ; 

=  {AUV_stbd_rpin_temp)  ; 

=  {AUV_bow_vertical_temp)  ; 

=  (AUV_stern_veri:ical_temp)  ; 

=  (AUV_bow_lateral_teinp)  ; 

=  {AUV_st:ern_lateral_temp)  ; 

=normalize (AUV_ST1000_bearing_tempi ; 

=  ( AUV_ST  100  0_rangG_t emp )  ; 

=  (AUV_ST1000_strength_teinp) ; 

=normalize(AUV__ST725_bearing_tGmp)  ; 

=  { AUV_ST7  2  5_range_temp )  ; 

=  (AUV_ST725_strength_temp)  ; 

=  (AUV_divetracker_rangel_temp)  ; 

=  (AUV_divetracker_range2_temp)  ; 


else  if  ( (variables_parsed  !=  STATEVECTORSIZE)  &&  (variablesjparsed  !=  -I)) 

strcpy  (command_buf fer,  passed_buf f er_ptr) ; 
parse_mission_script_commands  {) ; 


/*  if  (DISPLAYSCREEN)  printf  ("\nGarble  problem  in 
buf fer_received  !!!  variables  parsed  =  %d\n%s\n", 

variables_parsGd,  passed^buf  fer jtr)  ;  TRACE  =  TRUE;  */  } 


if  (TRACE  ScSc  DISPLAYSCREEN) 

{ 

printf  ("\nfrom  telemetry  buffer  state  variables:"); 
printf  ("\n  %s  t=%5.3f  x=%5.3f  y=%5.3f  z=%5.3f  ", 
keyword,  t , 

X.  y,  z); 

printf  {"phi=%5.3f  theta=%5.3f  psi=%5.3f  ", 

phi,  theta,  psi); 

printf  ("paddlewheel  speed=%5.3f  ", 
speed) ; 

printf  {"u=%5.3f  v=%5.3f  w=:%5.3f  p=%5.3f  q=%5.3f  r=%5.3f  ", 
u,  V,  w, 

P/  q.  r); 

printf  {"x_dot=%5.3f  y_dot=%5.3f  z_dot=%5.3f  ", 

x_do  t ,  y_do  t ,  z_do  t ) ; 

printf  ("phi_dot=%5.3f  theta_dot=%5 . 3f  psi_dot=:%5 , 3f  ", 
phi_dot,  theta_dot,  psi_dot); 

printf  ( "delta_rudder=%5 ,3f  delta jlanGs=%5 . 3f  ", 
delta_rudder,  deltas  lanes)  ; 

printf  ("port_rpm=%5.3f  stbd_rpm=%5 ..3f  ", 
port_rpm,  stbd_rpm) ; 

printf  ("bow_yertical=%5 .3f  stern_vertical=%5 . 3f  ", 

AUV_bow__vertical,  AUV_stern_vertical)  ; 
printf  ("bow_latGral=%5.3f  stern_lateral=%5 . 3f  ", 

AUV_bow_latGral ,  AUV__stern_lateral )  ; 
printf  ("STIOOO  b/r/s  %5.3f  %5.3f  %5.3f,  ST725  b/r/s  %5.3f  %5.3f  %5.3f", 
AUV_ST1 0  0  0_bear i ng ,  AUV_ST1 0  0  0_range ,  AUV_ST1 0  0  0_st  rength , 
AUV_ST725_bearing,  AUV_ST7 2 Strange,  AUV_ST7 2 5_st rength)  ; 

printf  ("divetrackGr_rangel=%5.3f  divetracker_range2=%5 . 3f  ", 
divetracker_rangel ,  divetracker_range2 ) ; 
printf  (",  (current  time  %d  %d  %d]  \n" , 

system_tmp->tm_hour,  system_tmp->tm_min,  system_tmp->tm_sec) ; 


/*  keep  all  telemetry  variables  in  degrees  */ 
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phi 

theta 

psi 

phi__dot 

theta_dot 

psi_dot 

P 

q 

r 

delta_rudder 

delta_planes 


normal ize2 
normali2e2 
normalize 
normalize2 
normal i2e2 
normal! ze2 
normalize2 
normal! 2e2 
normalize2 
normal! ze2 
normal! ze2 


(ph!  )  ; 

(theta) ; 

(Ps!  )  ; 
(phi_dot)  ; 
(theta_dot)  ; 
(ps!_dot) ; 

(P); 

(q)  ; 

(r)  ; 

(delta_rudder) ; 
(delta  _planes) ; 


if  (TRACE  ScSc  DISPLAYSCREEN)  printf  ("[finish  parse  telemetry  string 
()]\n"); 


return; 

} 


void  read_telemetry_string  () 


/*  this  function  reads  a  telemetry  string  from  a  telemetry  file  */ 

/*  temporary  hold  variables  */ 


double 


AUV_time_temp , 

AUV_x_temp, 

AUV_phi_t  emp , 

AUV_speed_t  emp , 

AUV_u_temp, 

AXJV_p_temp, 

AUV_x_do  t_temp , 
AUV_phi__dot_temp , 

AUV_del ta_rudder_temp , 
AUV_por  t_rpm__temp , 
AUV_bow_ver  tical_:temp , 
AUV_bow_l  a  t  er  al_t  emp , 
AUV_ST1 0 0 0_bear ing_temp , 
AUV_ST1 0  0  0_s  t  reng  t  h__t  emp , 
AUV_ST7  2  5_r  ange_t  emp , 
AUV__divetrack:er_rangel_temp 


AUV_/_temp , 
AUV_theta_temp , 


AUV__2_temp, 

AUV_psi_temp, 


AUV_v_temp , 

AUy_q_temp  / 

AUV_y_do  t_t  emp , 

AUV_  th  e  t  a__do  t_t  emp , 

AUV_de  1 1  a_pl  anes_t  emp , 

AUV_s  t  bd_rpm_t  emp , 

AUV_s  t  ern__ver  t  i  cai_t  emp , 
AUV_stern_lateral_temp , 
AUV_ST1 0  0  0_range_t  emp , 
AUV_ST7  2  5_bear  ing_temp , 
AUy_ST7  2  5_s  t  r  eng  t  h_t  emp , 

,  AUV_divetracker_range2_temp; 


AUV_w_temp, 
AUV_r_temp, 
AUV_z_do  t_t  emp , 
AXJV_p  s  i_do  t_t  emp , 


variablesjarsed  =  0; 


if  (TRACE  &&  DISPLAYSCREEN)  printf  ("[begin  read_telemetry_string  ()]\n"); 

/*  update  and  output  AUV  state  variables  .  */ 

/*  note  that  if  dead  reckoning  is  used,  values  will  not  change  */ 

/*  note  %F  required  by  OS-9,  accepted  by  SGI  as  equivalent  to  %lf  */ 


while  (  (variable's_parsed  !=  STATEVECTORSIZE)  &&  (variables_parsed  !=  -1)) 
variablesjarsed  =  fscanf  ( telemetry_f ile, 

#ifndef  os9 


"%s  %lf  %lf  %lf  %lf  %lf  %lf  %if  %if  %if  %if  %if  %if  %if  %if  %2f  % 
%lf  %lf  %lf  %lf  %lf  %lf  %lf  %lf  %lf  %lf  %lf  %if  %if  %if  %if  %if  %if  %if  %if 
%lf\n". 


if 


#else 


"%s  %F  %F  %F  %F  %F  %F  %F  %F  %F  %F  %F  %F  %F  %F  %F  %F  %F  %F  %F  %F  %F  %F 
%F  %F  %F  %F  %F  %F  %F  %F  %F  %F  %F  %F  %F  %F\n", 

#endif 


keyword, 
&AUV__x_temp , 
&:AUV_phi_temp , 
&:AUV_speed_t  emp , 
&:AUV_u_temp , 
&AUV_p_temp , 


&AUV_t  ime_t  emp , 
S:AUV_y_temp, 
&AUV_theta_temp , 

&AUV_v_temp, 
ScAUV_q_temp , 


&AUV_z_temp , 
£cAUV_p  s  i_t  emp , 

&AUV__w_t€mp, 

&AUV_r_temp, 
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&AUV_x_dot_teinp ,  &AUV_y_dot_teinp ,  5cAUV„2_dot_t  emp , 

&AUV_phi_dot_temp ,  ScAUV_theta_do  t_teinp ,  &AUV_psi_dot:_teinp , 

&AUV_del  ta_rudder_teinp ,  &AUV_deI  ta_planes_teTnp , 

&AUV_p  or  t_rpm_t  emp ,  5cAUV_s  t  bd_rpm_  t  emp , 

5cAUV_bow_ver  t  i  c  a  l_t  emp ,  &AUV_s  t  e  r  n_ve  r  t:  i  ca  t  emp , 

&AUV_bow_l ateral^temp,  & AUV_s  tern_lateral_t  emp , 

£cAUV_ST1000_bearing_temp,  ScAUV_ST1000_range_temp, 

&AUV_ST1 00  0_st  rength_temp , 

&AUV_ST7  2  5_bea  r  i  ng_t  emp ,  ScAUV_ST7  2  5__r  ange_t  emp , 

&:AUV_ST7  2  5_s  trengt  h_temp , 

&AUV_divetracker_rangel_temp,  &AUV_divetracker_range2_temp) ; 


if 

{ 


{variables_parsed  ==  STATEVECTORSIZE)  /*  transfer  OK,  keep  new  values 


t 

X 

y 

z 

phi 

theta 

psi 

speed 

u 

V 

w 

p 

q 

r 

x_dot 

y_dot 

z_dot 

phi_dot 

theta_dot 

psi_dot 

delta_r udder 

delta_planes 

port_rpm 

stbd_rpm 

AUV__bow_ver  t  i  ca  1 
AUV__s  t  e  rn__ver  t  i  cal 
AUV__bow_l  a  t  era  1 
AUV_s  t  e  rn_l a t  e r a 1 
AUV_ST1000_bearing 
AUV_ST1 0  0  0_range 
AUV^STl 0  0  0_s  t  reng th 
AUV_ST725_bearing 
AUV_ST725_range 
AUV_ST725_strGngth 
divetracker_rangel 
divetracker_range2 


=  {AUV_tiine_temp)  ; 

=  {AUy__x_teinp) ; 

=  (AUV_y_temp)  ; 

=  (AUV_z_temp) ; 

=  {AUV__phi_temp)  ; 

=  (AUV_theta_temp)  ; 

=  (AUV__psi_temp)  ; 

=  {AUV_speed_temp)  ; 

=  (AUV_u_temp)  ; 

=  (AUV_v_teinp) ; 

=  { AUV_w_  t  emp )  ; 

=  {AUV__p_temp) ; 

=  ( AUV_q_t  emp )  ; 

=  (AUV_r_temp) ; 

=  { AUV_x__do  t_t emp )  ; 

=  ( AUV_^_do  t_t  emp ) ; 

=  ( AUV_z_do  t_t emp )  ; 

=  { AUV_ph  i_do  t_  t  emp )  ; 

=  (AUV_theta_dot_temp)  ; 

=  .  (AUV__psi_dot_temp)  ; 

=  (AUV_delta_rudder_temp)  ; 

=  (AUV_delta_planes_temp)  ; 

=  (AUV_port_rpm_temp)  ; 

=  ( AUV_s  t  bd_r pra_t  emp )  ; 

=  (AUV_bow_vertical_temp)  ; 

=  {AUVl_stern_vertical_temp)  ; 

=  {AUV_bow_lateral__temp)  ; 

=  (AUV_stern_lateral_temp)  ; 

^normalize (AUV_ST1000__bearing_temp)  ; 

=  (AUV_ST1000_range_temp)  ; 

=  (AUy_ST1000_strength_tGmp); 

=:normalizG(AUV_ST725_bearing_temp)  ; 

=  (AUV__ST725_range_temp) ; 

=  (AUV_ST725_strength_tGmp)  ; 

=  {AUV_divetracker_rangel__temp)  ; 

=  (AUV_divetracker_range2_temp) ; 


else  end__test  =  TRUE; 


*/ 


if  (TRACE  &&  DISPLAYSCREEN) 

{ 

printf  ("\nRead  from  telemetry  file:"); 
printf  ("\n  %s  t=%5.3f  x=%5.3f  y=%5.3f  z=%5.3f  ", 
keyword,  t, 

X,  y,  2); 

printf  ("phi=%5.3f  theta=%5.3f  psi=%5,3f  ", 

phi,  theta,  psi); 

printf  ("paddlewheel  speed=%5.3f  ", 
speed) ; 

printf  ("u=%5.3f  v=%5.3f  w=%5.3f  p=%5.3f  q=%5.3f  r=%5.3f  ", 
u,  V,  w, 

P/  Q/  r) ; 

printf  ("x_dot=%5.3f  y_dot=%5.3f  z_dot=%5.3f  ", 

x_dot,  y_dot,  z__dot); 
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printf  ("phi_dot=%5.3f  theta_dot=%5 . 3f  psi_dot=%5 . 3f  ", 
phi_dot,  theta^dot,  psi_dot); 

printf  ("delta_rudder=%5 .3f  delta_planes=%5 . 3f , 
delta_rudder,  deltajplanes) ; 

printf  { "port_rpm=%5 .3f  stbd_rpm=%5 . 3f  ", 
port^rpm,  stbd^rpm) ; 

printf  ("bow_vertical=%5 .3f  stern_vertical=%5 . 3f  ", 

AUV_bow_vertical,  AUV_stern_vertical) ; 
printf  ("bow_lateral=%5. 3f  stern_lateral=%5 . 3f  ", 

AUV_bow_lateral ,  AUV_stern_lateral ) ; 
printf  ("STIOOO  b/r/s  %5.3f  %5.3f  %5.3f,  ST725  b/r/s  %5.3f  %5.3f  %5.3f" 
AUV_ST1 0  0  0_bea  r i ng ,  AUV_ST1 0  0  0_range ,  AUV_ST1 0  0  0_s  t  reng th , 
AUV_ST725_bearing,  AUV_ST725_range,  AUV_ST725_strength) ; 
printf  { "divetracker_rangel=%5 . 3 f  divetracker__range2=%5 . 3f  ", 
divetracker_rangel ,  divetracker_range2) ; 
printf  (",  [current  time  %d  %d  %d]  \n", 

system_tmp'->tm_hour,  system_tmp''>tm_min,  system_tmp->tm_sec)  ; 

} 

/*  keep  all  telemetry  variables  in  degrees  */ 
phi  =  normalize2  (phi  ) ; 

theta  =  normalize2  (theta); 

psi  =  normalize  (psi  ) ; 

phi_dot  =  normalize2  (phi_dot); 

theta_dot  =  normalize2  {theta_dot) ; ’ ' 

psi_dot  =  normalize2  (psi_dot); 

p  =  normalize2  (p) ; 

q  =  normalize2  (q) ; 

r  =  nomalize2  (r) ; 

delta_rudder  =  norma lize2  (delta_rudder) ; 
delta_planes  =  normalize2  {delta_planes) ; 

if  (TRACE  &&  DISPLAYSCREEN)  printf  {"[finish  read_telemetry_string  ()]\n") 
return; 

} 

/*************************************************★*************************/ 

void  open_tactical_socket  {}  /*  see  os9sender,c  for  original  code  */ 

{ 

if  (TRACE  ScSc  DISPLAYSCREEN) 

printf  ("[start  open_tactical_socket  {)]\n"); 


/*  Initialize  communications  blocks  */ 

/*  Initialize  both  client  &  server  ************★***************************/ 

/*  Signal  handlers  for  termination  to  override  net_open  ()  and  net_close  ()*/ 
/*  signal  handlers.  Otherwise  you  are  unable  to  "^C  kill  this  program.  */ 

#ifndef  os9 

signal  (SIGHUP,  shutdown_tactical_socket ) ; /*  hangup  */ 

signal  (SIGINT,  shutdown_tactical_socket ) ; /*  interrupt  character  */ 

signal  (SIGKILL,  shutdown_tactical_socket ) ; /*  kill  signal  from  Unix  */ 

signal  (SIGPIPE,  shutdown_tactical_socket ) ; /*  broken  pipe  from  other  host*/ 

signal  (SIGTERM,  shutdown_tactical_socket ) ; /*  software  termination  */ 

#endif 

/*  Initialize  sender  ******************************************************/ 

/*  start  by  finding  default/desired  remote  host  to  connect  to  */ 

{ 


server_entity  =  gethostbyname  ( tactical_remote_host_name)  ; 
if  (server_entity  ==  NULL) 

{ 

if  (DISPLAYSCREEN) 

{ 
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printf (" (open_tactical_socket :  tactical  remote  hostXn"); 
printf("  {\''%s\")  not  foundJXn", 

tactical_reinote_host_name)  ; 

fflush  (stdout) ;  /*  force  completion  of  screen  write  */ 

} 

/*  error  message  needed  on  (open)  output  file  <<<<<<<<<<<<<<<<<<<  */ 
tactical_socket_opened  =  FALSE; 
exit  (1); 

} 

else  if  (TRACE  &&  DISPLAYSCREEN) 

{ 

printf {" [open_tactical_socket:  tactical  remote  host  "); 
printf (" (X"%sX" )  located] Xn",  tactical_remote_host_name) ; 

} 

/*  Client  opens  server  port  ***********************************:«:*****/ 

/*  Fill  in  structure  'serve readdress'  with  the  address  of  the  */ 

/*  remote  host  (i.e.  SERVER)  that  we  want  to  connect  with:  */ 

#ifdef  sgi 

bzero  ((char  *)  &server_address,  sizeof  (server_address) ) ; 

#endif 

server_address.sin_family  =  AF_INET;  /*  .Internet  protocol  family  */ 
/*  copy  server  IP  address  into  sockaddr_in  struct  server_address  */ 

#ifdef  sgi 

bcopy  (server_entity->h_addr,  & (serve readdress . si n_addr . s_addr) , 
server_entity->h_length)  ; 

#else 

strncpy ( (char  *) & (server_address , sin_addr . s_addr) , server_entity->h_addr, 
server_entity->h_length)  ; 

#endif 

/*  make  sure  port  is  in  network  byte  order  */ 

server_address.sin_port  =  htons  (AUVSIMl_TCP_PORT_2 )  ; 

/*  Open  TCP  (Internet  stream)  socket  */ 

if  (  (tactical_socket_descriptor  =  socket  (AF_INET,  SOCK__STREAM,  0))  <  0 

) 

{ 

if  (DISPLAYSCREEN) 

{ 

printf  (" [open_tactical_socket :  client  can't  open  server"); 
printf  ("  tactical  stream  socket]"); 

} 

/*  error  message  needed  on  (open)  output  file  <<<<<<<<<<<<<<<<<<<  */ 
tactical_socket_opened  =  FALSE; 
exi t  ( 1 ) ; 

} 

else  if  (TRACE  &&  DISPLAYSCREEN) 

{ 

printf  ("  [open_tactical__socket :  client  opened"); 
printf  ("  tactical  server  socket  successfully ] Xn" ) ; 


/*  Connect  to  the  server.  Process  will  block/sleep  until  connection  is 
is  established.  Timeout  will  return  an  error.  */ 

if  (connect  (  tactical_socket_descriptor, 

(struct  sockaddr  *)  &server_address, 

sizeof  (server_address) )  <  0) 

{ 

if  (DISPLAYSCREEN) 

{ 

printf  { " [open_tactical_socket :  client  can't  connect  to"); 
printf  ("  tactical  server  socket,  tactical  comms  ignored] Xn"); 
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/ 


/*  error  message  needed  on  (open)  output  file  <<<<<<<<<<<<<<<<<<<  * 
tactical_socket_opened  =  FALSE; 

if  ^  (LOCATIONLAB)  exit  (1)  ; 

else  if  (DISPLAYSCREEN)  printf  ("[continuing  anyway]"); 

else 
•  { 

if  (TRACE  &&  DISPLAYSCREEN) 

{ 

printf  ("[execution  client  connected  to  tactical"); 
printf  ("  server  socket  successfully ] \n" ) ; 

tactical_socket_opened  =  TRUE; 

} 

}  /*  end  initialization  */ 

tactical_socket_stream  =  tactical_socket_descriptor;  /*  client  */ 

if  (TRACE  &&  DISPLAYSCREEN)  /*  print  final  info  */ 

{ 

printf (" [open_tactical_socket  CLIENT:  tactical_socket_descriptor  =  %d]\n", 

tactical_socket_descriptor) ; 
printf ("[  tactical_socket_accepted  =  %d]\n", 

tactical__socket__accepted)  ; 

printf  ("[  tactical_socket_streain  =  %d]\n", 

tactical_socket_stream) ; 


if  (TRACE  &&  DISPLAYSCREEN) 

printf  ("[finish  open_tactical_socket  ()]\n"); 

return; 

}/*  end  open_tactical_socket  ()  */ 

/******************************************************************** *****^*^ 

void  shutdown_tactical_socket  ()  /*  see  osSsender.c  for  original  code  */ 

int  kill_return_value; 

shutdown_signal_received  =  TRUE; 

if  ( tactical_socket_opened  ==  FALSE) 

{ 

if  (TRACE  &&  DISPLAYSCREEN) 

{ 

printf  {" [tactical_socket_opened  FALSE, ") ; 
printf  ("  shutdown_tactical_socket  ignored] \n" ) ; 

} 

return; 

} 

if  (TRACE  ScSc  DISPLAYSCREEN) 

printf  (" [shutdown_tactical_socket  start  . . . ] \n") ; 

/*  No  need  to  send  a  message  to  other  side  that  bridge  is  going  down,  */ 

/*  since  SIGPIPE  signal  trigger  may  shutdown  server  on  other  side  */ 

if  (close  (tactical_socket_stream)  ==  -1) 

{ 

if  (TRACE  ScSc  DISPLAYSCREEN) 

printf  ("shutdown_tactical_socket  close  ( tactical_socket_stream)  failed\n") ; 
/*  shutdown  {)  reference:  "Using  OS~9  Internet"  manual  p,  2-55  */ 

if  (shutdown  ( tactical_socket_stream,  2)  ==  -1) 
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{ 

if  (TRACE  &&  DISPLAYSCREEN) 

{ 

printf  ( "  (s]iutdown_tactical_socket  shutdown"); 
printf  ("  (tactical_socket_stream,  2)  failed]  \n"); 


kill_return__value  =  kill  { tactic al_socket_st ream,  SIGKILL) ; 

if  (TRACE  ScSc  DISPLAYSCREEN) 

{ 

printf  (" [shutdown_tactical_socket  kill  (tactical_socket_stream, ") ; 
printf  ("  SIGKILL)  returned  %d]\n",  kill_return_value) ; 

} 

} 

if  (TRACE  &&  DISPLAYSCREEN) 

printf  (" [shut down_t act ical_so eke t  return] \n"); 

return; 


)  /*  end  shutdown_tactical_socket  ()  */ 


void  send_buffer_to_tactical_socket  ()  /* 
if  (HALTSCRIPT)  return; 


see  os9sender.c  for  orig. 


code  */ 


bytes_left  =  socket_length; 

bytes_written  =  0; 

ptr_index  =  buffer;  /*  this  global  string  is  the  data  to  be  sent  */ 

if  (tactical__socket_opened  ==  FALSE) 

if  (TRACE  &&  DISPLAYSCREEN) 

{ 

printf  (" [send_buffer_to_tactical_socket ;  ") ; 

^  printf  ("tactical_socket_opened  ==  FALSE,  returning] \n") ; 

return; 

} 

if  (TRACE  &&  DISPLAYSCREEN) 

printf  (" [send_buf fer_to„tactical__socket  start  ...]\n"); 

while  (  (bytes_lef t  >  0)  &&  (bytes_written  >=  0))  /*  write  loop  ***********/ 

bytes_sent  =  write  ( tactical_socket_stream,  ptr_index,  bytes_left) ; 

if  (bytes_sent  <  0)  bytes_written  =  bytes_sent; 

else  if  (bytes_sent  >  0) 

{ 

bytes_left  -=  bytes_sent; 
t>ytes__written  +=  bytes_sent; 
ptr_index  +=  bytes_sent; 

} 

if  (LOCATIONLAB  &&  TRACE  &&  DISPLAYSCREEN) 

printf  ( " [record_data  send_telemetry_to_server  loop" ) ; 
printf  ("  bytes  sent  =  %d]\n",  bytes_sent); 

} 

if  (bytes__written  <  0) 

{ 


HALTSCRIPT  =  TRUE;  /*  loss  of  socket  comms  with  tactical  level  */ 
if  (LOCATIONLAB  &&  DISPLAYSCREEN) 
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{ 


printf  (" [record_data  send_t el emetry_t observer  ()  send  failed,  "); 
printf  ("%d  bytes^written]  \n'' ,  byt:es_wri tten) ; 

} 

/*  error  message  needed  on  (open)  output  file  «««««««««  */ 

else  if  (LOCATIONLAB  &&  TRACE  &&  DISPLAYSCREEN) 

{ 

printf  ( " [record_data  send__t el erne try_t observer  total  bytes  sent") 
printf  {"  =  %d]\n",  bytes_written) ; 

} 

/*  Check  termination  **********************************★*★*****★********/ 

if  (strncmp  (buffer,  "shutdown",  8)  ==  0) 

{ 

if  (TRACE  &&  DISPLAYSCREEN)  printf 

(" [ send_buf f er_to_tactical_socket :  shutdown's ignal_received] \n") ; 
shutdown_tactical_socket  ( ) ; 

} 

if  (TRACE  ScSc  DISPLAYSCREEN) 

printf  (" [send_buffer_to_tactical_socket  return] \n") ; 

return; 

}  /*  end  send_buffer_to_tactical_socket  ()  */ 

/************************************************ **^* **** *^ ****** ^^^^^^^^^^^^ 

void  get_string_f rom_tactical_socket  ()  /*  see  os9sender.c  for  original  */ 

if  (tactical_socket_opened  ==  FALSE) 

{ 

if  (TRACE  BcSc  DISPLAYSCREEN) 

printf  (" [get_string_from_tactical_socket  not  open,  ignored] \n" ) ; 
return; 

} 

if  (TRACE  Sc&  DISPLAYSCREEN) 

printf  ("  [get_string_from_tactical_socke't  start  .  .  .  ]  \n")  ; 

./ 

/*  listen  to  remote  host,  relay  to  local  network /program  */ 

bytes_left  =  socket_length; 

bytes_received  =  0; 

ptr_index  =  command_buf f er ;  /*  command_buf fer  is  where  results  go  */ 

while  ( (bytes_lef t  >  0)  &&  (bytes_received  >=  0))  /*  read  loop  *********/ 

{ 

bytes_read  =  read  (tactical_socket_stream,  ptr_index,  bytes_left)  ; 

if  (bytes_read  <  0)  bytes_receiyed  =  bytes_read; 

else  if  (bytes_read  >  0) 

{ 

bytes_left  -=  bytes_read; 
bytes_received  +=  bytes_read; 
ptr_index  +=  bytes^read; 

} 

if  (TRACE  ScSc  DISPLAYSCREEN) 

{ 

printf  (" [get_string_from_tactical_socket  receiver  block"); 

^  printf  ("  loop  bytes_read  =  %d]\n",  bytes_read) ; 

/*  if  nothing  is  waiting  to  be  read,  break  out  of  read  loop  */ 

^  if  ( (bytes_read  ==  0)  &&  (bytes_received  ==  0))  break; 

if  (bytes_received  <  0)  /*  failure  */ 

{ 

if  (TRACE  &&  DISPLAYSCREEN) 
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{ 

printf  (" [get_string_froin_tactical_socket  receiver  block  read"); 
printf  {"  failed,  bytes_received  =  %d\n",  bytes_received) ; 

} 

else  if  (bytes_received  ==  0)  /*  no  transfer  */ 

{ 

if  (TRACE  &&  DISPLAYSCREEN) 

{ 

printf ( " [get_string_f ronutactical_socket  received  0  bytes ! i ] \n" ) ; 


else  if  (bytes_received  >  0)  /*  success  */ 

{ 

if  (TRACE  DISPLAYSCREEN) 

{ 

printf {" [get_string_froin_tactical_socket  received  %d  bytes] \n", 
bytes_received) ; 

} 

} 

/*  Check  termination  ****************************************************/ 

if  {strncmp  (command_buf fer,  "shutdown",  8)  ==  0) 

if  (TRACE  &&  DISPLAYSCREEN)  printf 

( " [get_data__on_tactical_socket :  shutdown_signal_recei ved] \n" ) ; 
shutdown__tactical_socket  ( )  ; 

} 

if  (TRACE  ScSc  DISPLAYSCREEN) 

printf  ( "  [get_string_f roin_tactical_socket  finish]  \n" ) ; 
return; 


}  /*  end  get_string_froin_tactical_socket  ()  */ 


void  record_data_on  () 

{ 

if  (TRACE  &Sc  DISPLAYSCREEN)  printf  ("[start  record_data_on  ()]\n"); 

/*  Open  files  for  writing  */ 

if  ( (TACTICALPARSE)  I  I  (TACTICAL  ==  FALSE)) 

if  ( (auvdatafile  =  fopen  (AUVDATAFILENAME, "w" ) )  ==  NULL) 

printf ( "record_data_on  ()  unable  to  open  output  file  %s  for  writing.", 
AUVDATAFILENAME) ; 

printf 

("  Check  ownership  permissions  in  current  directory . \n" ) ; 

printf ("Exit . \n" ) ; 
exit  ('!); 

} 

if  (TRACE  &&  DISPLAYSCREEN  &&  (auvdatafile  !=  NULL)) 
printf  ("[auvdatafile  %s  open,  pointer  =  %x]\n", 

AUVDATAFILENAME,  auvdatafile); 


i f  ( SONARINSTALLED ) 

{ 

if  ( { (stlOOOdatafile  =  fopen  (STIOOODATAFILE, "w" ) )  ==  NULL)  M 
^  ( (targetdatafile  =  fopen  (TARGETDATAFILE, "w" ) )  ==  NULL)) 

printf ("record_data_on  ()  unable  to  open  stlOOO  data  file  %s  or  ", 
STIOOODATAFILE) ; 

printf ("target  data  file  %s  for  writingXn" , TARGETDATAFILE) ; 
printf 

("  Check  ownership  permissions  in  current  directory .\n"); 


99 


} 


printf ("Exit.Xn"); 
exit  (-1); 

} 

else 


{ 

printf ( " [stlOOOdataf ile  %s  open, 
STIOOODATAFILE, 

printf (" [targetdatafile  %s  open, 
TARGETDATAFILE, 

} 


pointer  =  %x]\n", 
StlOOOdataf ile) ; 
pointer  =  %x] \n" , 
targetdatafile) ; 


if  ( (TACTICALPARSE)  II  {TACTICAL  ==  FALSE)  li  (auvdatafile  !=  NULL)) 

{ 

fprintf  (auvdatafile,  "#  auvdatafile  %s  shows  %d  ", 
AUVDATAFILENAME,STATEVECTORSIZE)  ; 

fprintf  {auvdatafile,  "state  vector  variables  at  %3.1f  intervals . \n\n", 
dt)  ; 


fprintf  {auvdatafile, 

"#  state 

fprintf  {auvdatafile,  " 

fprintf  {auvdatafile,  "  phi  theta  psi 

fprintf  {auvdatafile,  "delta  delta  port  stbd  "); 


paddle  "); 


fprintf  {auvdatafile, 
f p  r i nt  f  { auvda  ta  f i 1 e , 
fprintf  (auvdatafile, 
fprintf  (auvdatafile, 
fprintf  {auvdatafile. 


"bow_  stern  bow_  stern 

"_ST1000  sonar _  "); 

"__ST725  sonar _  "); 

"Dive  Dive" )  ; 

"\n")  ; 


fprintf  (auvdatafile, 

"#  vector  t  X  y  z  phi  theta  psi 

fprintf  (auvdatafile,  "u  v  w  p  q  r 

fprintf  (auvdatafile,  "x_dot  y_dot  z_dot  _dot  _dot  __dot 

fprintf  (auvdatafile,  "rudder  plane  rpm  rpm  ")  ; 


speed  " )  ; 
")  ; 

")  ; 


fprintf  (auvdatafile, 
fprintf  (auvdatafile, 
fprintf  (auvdatafile, 
f pr i nt  f  { auvda  ta  file, 
f p  r i n t  f  ( auvda  t a  f i 1 e , 
} 


"vrtcl  vrtcl  latrl  latrl 
"bng  range  dB  "); 

"bng  range  dB  " )  ; 
"Trkl  Trk2"); 

''\n\n")  ; 


if  ( (auvtextfile  =  fopen  (AUVTEXTFILENAME, "w" ) )  ==  NULL) 

{ 

printf ("record_data_on  ()  unable  to  open  output  file  %s  for  writing.  ", 
AUVTEXTFILENAME) ; 

printf 

("  Check  ownership  permissions  in  current  directory . \n") ; 

printf ("Exit . \n") ; 
exit  (-1); 

} 

if  (TRACE  &&  DISPLAYSCREEN) 

printf  {"[auvtextfile  %s  open,  pointer  =  %x]\n", 

AUVTEXTFILENAME,  auvtextfile); 


fprintf  (auvtextfile,  "#  auvtextfile  %s  shows  %d  ", 
AUVDATAFILENAME,STATEVECTORSIZE)  ; 

fprintf  (auvtextfile,  "state  vector  variables  at  %3.1f  intervals . \n\n" , 
dt)  ; 


fprintf  (auvtextfile, 

"#  state 

fprintf  (auvtextfile,  " 

fprintf  (auvtextfile,  "  phi  theta  psi 

fprintf  (auvtextfile,  "delta  delta  port  stbd  "); 


paddle  " ) ; 
"); 

)  ; 
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fprintf  (auvtextfile,  "bow_  stern  bow__  stern  "); 

fprintf  (auvtextfile,  "_ST1000  sonar _  "); 

fprintf  (auvtextfile,  "_ST725  sonar _  "); 

fprintf  (auvtextfile,  "Dive  Dive"); 
fprintf  (auvtextfile,  "\n"); 

fprintf  (auvtextfile, 

"#  vector  t  X  y  z  phi  theta  psi  speed  ") ; 

fprintf  (auvtextfile,  "u  v  w  p  q  r  "); 

fprintf  (auvtextfile,  "x_dot  y_dot  z_dot  __dot  _dot  _dot  "); 

fprintf  (auvtextfile,  "rudder  plane  rpm  rpm  "); 

fprintf  (auvtextfile,  "vrtcl  vrtcl  latrl  latrl  "); 

fprintf  (auvtextfile,  "bng  range  dB  "); 

fprintf  (auvtextfile,  "bng  range  dB  "); 
fprintf  (auvtextfile,  "Trkl  Trk2"); 
fprintf  (auvtextfile,  "\n\n"); 

if  (LOOPFOREVER) 

fprintf  (auvtextfile,  "#  Mission  replication  #%d\ri", 

replication_count )  ; 

/*  testing  code  from  wr2tl,c,  not  currently  in  use  */ 

/*  serial. d  is  a  telemetry  test  file  to  check  connectivity  */ 

/*  if  ( (serialtestfile  =  fopen  ("serial. d",  "r"))  <=  0) 

{ 

printf ("record_data_on  ()  can't  open  test  file  serial.d\n"); 
printf ("Exit.\n") ; 
exit  (-1); 

} 

*/ 

if  (TRACE  &&  DISPLAYSCREEN)  printf  ("[finish  record_data_on  ()]\n"); 
return; 

} 

/*************************************★*************************************  yr 

void  record_data_of f  () 

{ 

if  (TRACE  &&  DISPLAYSCREEN)  printf  ("[start  record_data_of f  {)]\n"); 

if  ( (auvdatafile  !=  NULL)  &&  TRACE  &&  DISPLAYSCREEN) 

{ 

printf  ("[flushing  and  closing  auvdatafile  %s  %x]\n", 

AUVDATA FILENAME,  auvdatafile) ; 
f flush  (stdout)  ;  /*  force  completion  of  screen  write 
) 

if  (auvdatafile  !=  NULL) 

{ 

if  (TRACE  &&  DISPLAYSCREEN)  printf  ("[auvdatafile  f lushed] \n" ) ; 
f  flush  (stdout);  /*  force  completion  of  screen  write  */ 
f  flush  (auvdatafile)  ;  /*  force  completion  of  file  write*/ 
fclose  (auvdatafile) ; 

if  (TRACE  &&  DISPLAYSCREEN)  printf  {"[auvdatafile  closed] \n"); 
f flush  (stdout) ;  /*  force  completion  of  screen  write  */ 

} 

else  if  {{TRACE  &&  DISPLAYSCREEN)  && 

( (TACTICAL  ==  FALSE)  1 |  (LOCATIONLAB) ) ) 
printf  (" [auvdatafile  was  not  open! 1 ] \n") ; 

if  (stlOOOdataf ile  1=  NULL) 

{ 

if  (TRACE  &&  DISPLAYSCREEN)  printf  (" [stlOOOdataf ile  f lushed] \n" ) ; 
f  flush  (stdout);  /*  force  completion  of  screen  write  */ 
f  flush  (stlOOOdatafile) ;  /*  force  completion  of  file  write  */ 
fclose  (stlOOOdatafile) ; 
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fflush  (targetdataf lie) ; 
fclose  (targetdataf lie) ; 
if  (TRACE  &&  DISPLAYSCREEN) 

printf  ( " [ stlOOOdataf ile  and  targetdataf i le  closed] \n"); 
fflush  (stdout);  /*  force  completion  of  screen  write  */ 

} 

if  (TRACE  &&  DISPLAYSCREEN) 

{ 

printf  ("[flushing  and  closing  auvtextfile  %s  %x]\n", 

AUVTEXTFILENAME,  auvtextfile) 
fflush  (stdout);  /*  force  completion  of  screen  write  */ 

} 

if  (auvtextfile  !=  NULL) 

{ 

if  (TRACE  ScSc  DISPLAYSCREEN)  printf  ("[auvtextfile  f  lushed]  \n" )  ; 
fflush  (stdout);  /*  force  completion  of  screen  write  */ 
fflush  (auvtextfile);  /*  force  completion  of  file  write  */ 
fclose  (auvtextfile) ; 

if  (TRACE  &&  DISPLAYSCREEN)  printf  ("[auvtextfile  closed] \n"); 
fflush  (stdout) ;  /*  force  completion  of  screen  write  */ 

} 

else  if  (TRACE  &&  DISPLAYSCREEN) 

printf  {"[auvtextfile  was  not  open!l]\n"); 

/*  fclose  (serialtestfile) ;  /*  serial  port  test  file  */ 

if  (TRACE  &&  DISPLAYSCREEN) 

{ 

printf  ("[finish  record__data_of f  ()]\n"); 

fflush  (stdout);  /*  force  completion  of  screen  write  */ 

} 

return; 


int  detect_death_spiral  () 

{ 

static  int  turn_di  recti  on  =  ec¬ 
static  double  psi_old  =  0.0; 

static  double  cumulative_turn  =  0.0; 

if  (TRACE  &&  DISPLAYSCREEN)  printf  ("[start  detect_death_spiral  ()]\n"); 

/*  reset  static  variables;  don't  check  for  spiral  */ 
if  {DEATH_SPIRAL_RESET) 

{ 

turn_,di  recti  on  =  0; 
cumulative__turn  =  0.0; 

DEATH_SPIRAL__RESET  =  FALSE; 
if  (TRACE  &&  DISPLAYSCREEN) 

printf  {"[finish  detect_death_spiral  ()]\n"); 
return  (FALSE) ; 

} 

/*  Turn  direction  changed,  reset  static  variables,  or  not  at  depth  yet  */ 
if  ((dsign  {psi_dot)  !=  turn_di recti on)  M  (turn__di recti on  ==  0)  il 
(fabs  (depth_error)  >  standoff_di stance ) ) 

{ 

turn_di recti on  =  dsign  (psi_dot); 
cumulative_turn  =  0.0; 
psi_old  =  psi; 
if  (TRACE  SeSe  DISPLAYSCREEN) 

printf  {"[finish  detect_death_spiral  ()]\n"); 
return  (FALSE); 
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} 


/*  Same  turn  direction,  check  for  full  circle  */ 

cumulative_turn  +=  normalize2  (psi  ~  psi__old) ; 
psi_old  =  psi; 

/*  Full  360  Degree  Constant  Direction  Turn  Means  Death  Spiral  */ 
if  ( {cumulative^turn  >=  360)  li  (cumulative_turn  <=  -360)) 

{ 

if  (TRACE  ScSc  DISPLAYSCREEN) 

{ 

printf  ("[Death  Spiral  Detected] \n" ) ; 
printf  ("[finish  detect_death_spiral  ()]\n"); 

} 

return  (TRUE) ; 

} 

/*  No  Spiral  Detected  */ 

if  (TRACE  ScSc  DISPLAYSCREEN)  printf  ("[finish  detect_death_spiral  ()|\n"); 
return  (FALSE); 

}  /*  end  int  detect_death_spiral  ()  */ 


void  cage_dg  ()  /*  dg  =  directional  gyro  */ 


/*  Low  TRUE  Logic  */ 

/*  Setting  (Cage  DG)  Low  and  (UnCage  DG)  High  will  Cage  the  DG  */ 
via0b_reg  =  viaOb^reg  &  OxFE;  /*  Set  bits  PBO  Low  retaining  */ 

/*  other  bits  */ 

viaO [ORB_IRB]  =  via0b_reg; 

/*via0b_reg  =  viaOb_reg  !  0x02;*/  /*  Set  bit  PBl  High  retaining  */ 

/*  other  bits  */ 

/*  Using  PB3  Pin  44/19  since  48/23  crapped  out  */ 

via0b_reg  =  via0b_reg  I  0x08;  /*  Set  bit  PB3  High  retaining  */ 

viaO [ORB_IRB]  =  via0b_reg; 

if  (DISPLAYSCREEN)  printf ( "Wai ting  20  sec.  for  Gyro  to  CageXn"); 
tsleep (2000) ;  /*  Wait  20  seconds  MAX  for  Caging  */ 

return; 

}  /*  end  cage_dg  ()  */ 

/***************************************************************************/ 
void  uncage_dg  ()  /*  dg  =  directional  gyro  */ 

{ 

/*  Low  TRUE  Logic  */ 

/*  Setting  (Cage  DG)  Hi  and  (UnCage  DG)  Low  will  UnCage  the  DG  */ 

via0b_reg  =  via0b„reg  1  0x01;  /*  Set  bit  PBO  High  retaining  */ 

/*  other  bits  */ 

viaO [ORB_IRB j  =  via0b_reg; 

/*via0b_reg  =  via0b_reg  &  OxFD;*/  /*  Set  bits  PBl  Low  retaining  */ 

/*  other  bits  */ 

/*  Using  PB3  Pin  44/19  since  48/23  crapped  out  */ 

via0b_reg  =  via0b_reg  &  0xF7;  /*  Set  bits  PB3  Low  retaining  */ 

viaO  [ORB_IRB]  =  via0b_reg; 
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tsleep(lOO);  /*  Wait  1  second  for  UnCaging  */ 
return; 

}  /*  end  uncage_dg  {)  */ 


double  dsign  (value) 
double  value; 

{ 

if  ((value  ==0.0)  M  (value  ==  -0.0))  return  (  1.0); 
if  (value  >0.0)  return  (  1.0); 

if  (value  <  0.0)  return  (-1.0); 

) 


double  dtanh (value) 
double  value; 

{ 

if (fabs (value)  >  1.0) 

{ 

return (dsign (value) ) ; 

} 

else 

{ 

return (value) ; 

} 


/*  double  epv,einv; 

epv  =  exp(value); 
emv  =  exp (-value); 

return  (  (epv  -  emv)  /  (epv  +  emv)  ); 

*/ 

} 
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/**********************************★* **********************************^****^ 
/* 

Program:  parse_functions .c 

Authors:  Don  Brutzman,  Mike  Burns,  Brad  Leonhardt,  Duane  Davis 

Revised:  2  August  96 

System:  AUV  Gespac  68020/68030,  OS-9  version  2.4 

Compiler:  Gespac  cc  Kernighan  &  Richie  (K&R)  C 

Compilation:  ftp>  put  parse_functions . c 

auvsiml>  chd  execution 

[68020]  auvsiml>  make  -k2f  execution 

[68030]  auvsiml>  make  execution 

[Irix  ]  fletch>  make  execution 

Purpose:  Reduce  size  of  execution. c  to  allow  OS-9  C  compiler  to  work 

Parse  command  line,  mission  script  commands,  control  constants 

Bugs:  Bizarre  vehicle  control  where  thrusters  alternate  directions 

with  no  apparent  progress  is  a  symptom  of  incorrect  control 
constant  initialization.  Corrected. 

Note  that  %F  double  formats  are  used  instead  of  %lf  on  scanf{)  and  sscanf  () 
calls  for  OS-9  compatibility.  SGI  C  compiler  does  not  complain, 
gcc  on  Sun  does  fail  at  run-time,  so  ifdef's  are  used  to  support 
the  proper  format  string.  (Curse  you  OS-9!  :) 

*/ 

/**★************************************************************************ ^ 
/*  parse_functions,c  */ 

#include  "globals.h" 

#include  "statevector .h" 

# include  "defines. h" 

/**★******************★*★************************************************** ^ 


void  parse_command_line_f  lags  (); 
int  parse_mission_script_commands  () ; 
void  parse_mission_string_commands  (); 

void  print_valid_keywords  (); 

void  get_control_constants  (); 

extern  int  detec t_death_spiral  (); 
extern  void  send_buf fer_to_virtual_world_socket  (); 
extern  void  clamp  (); 
extern  double  normalize  (); 
extern  double  normali2e2  () ; 
extern  double  dsign  (); 


char  new_filename  [160]; 

char  backupcommand  [160]; 

int  system_result ; 

/***************************★*★*********************************************/ 
void  parse_command_line_f lags  (argc,  argv) 

int  argc;  char  **argv;  /*  command  line  arguments  */ 

{ 
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int  index; 
if  (DISPLAYSCREEN) 

{ 

printf  ("\n[parse_coinmand_line_flags  start:  #  arguments  =  %d]\n[", 
argc) ; 

for  (i  =  0;  i  <  argc;  i++)  printf  {"  %s" ,  argv[i]); 
printf  ("  ]\n"); 

} 


if  (DISPLAYSCREEN)  printf  {"[parse  arguments:  "); 

{ 

for  (i  =  1;  i  <  argc;  i++) 

( 

printf  ("%s  ",  argv[i]); 


} 


if  { (strcmp  (argv[i-l] , "TELEMETRY" )  !=  0)  && 

(strcmp  (argv[i-l] , "TELEMETRY-FILE")  !=  0)  && 

(strcmp  {argv[i-l] ,"TELEMETRYFILE")  !=0)  && 

(strcmp  (argv[i-l] /'MISSION")  U  0)  && 

(strcmp  (argv[i-l] /'SCRIPT")  0)  && 

(strcmp  (argv[i-l] /'FILE")  ‘  !=  0)  && 

(strcmp  (argv[i-l] /'FILENAME" )  i=  0)) 


for  (index  =  0;  index  <=  (int)strlen  (argv[i]);  index++) /^uppercase*/ 
argv[i)  [index]  =  toupper  (argv[i ]  [index]); 

printf  ("]\n"); 


strcpy  (buffer,  "");  /*  initialize  for  SILENT  */ 


for  (i  =  1;  i  <  argc;  i++) 

{ 

if  ((strcmp  {argv[i], 

(strcmp  (argvii], 
(strcmp  (argv[i], 
(strcmp  (argvii], 

{ 


"HELP")  ==  0)  II 
"7")  ==  0)  II 
"/?")  ==  0)  11 
"-?")  ==  0)) 


if  (TRACE  &&  DISPLAYSCREEN)  printf  ( " [print^help ] 
print_help  =  TRUE; 

} 

else  if  ((strcmp  {argv[i],  "KEYBOARD")  ==  0)  li 

(strcmp  (argv[i],  "KEY-BOARD")  ==  0)  II 

(strcmp  (argv[i],  "KEYBOARD- INPUT" )  ==  0)  I! 
(strcmp  (argv[i],  "KEYBOARD INPUT")  ==  0)) 

{ 


// 


); 


if  (TRACE  &&  DISPLAYSCREEN)  printf  (" [KEYBOARD INPUT  =  TRUE] 
KEYBOARDINPUT  =  TRUE; 


) 

else  if  (strcmp  (argv[i],  "TRACE")  ==  0) 

{ 

if  (TRACE  £=&  DISPLAYSCREEN)  printf  ("[TRACE  =  TRUE]  "); 
TRACE  =  TRUE; 

} 

else  if  ((strcmp  (argv[i],  "TRACEOFF")  ==  0)  II 

(strcmp  (argv(i),  "TRACE-OFF")  ==  0)  II 

(strcmp  (argv[i],  "NOTRACE")  ==  0)  II 

(strcmp  (argv(i],  "NO-TRACE")  ==  0)) 

{ 

if  (TRACE  &&  DISPLAYSCREEN)  printf  ("[TRACE  =  FALSE)  " ) ; 
TRACE  =  FALSE; 

} 

else  if  ((strcmp  (argv[i],  "LOOPFOREVER" )  ==  0)  I  I 
(strcmp  (argv[i],  "LOOP-FOREVER")  ==  0)) 

{ 

if  (TRACE  &&  DISPLAYSCREEN)  printf  ("[LOOPFOREVER]  "); 
LOOPFOREVER  =  TRUE; 

} 

else  if  ((strcmp  (argv[i],  "LOOPONCE")  ==  0)  II 


// 


); 
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(strcmp  (argv[i],  "LOOP-ONCE")  ==  0)) 

{ 

if  (TRACE  &&  DISPLAYSCREEN)  printf  ("[LOOPONCE]  ") ; 

LOOP FOREVER  =  FALSE; 

} 

else  if  ({strcmp  {argv[i],  "LOOPFILEBACKUP" )  ==  0)  II 
(strcmp  (argv[i],  "LOOP- FILE-BACKUP" )  ==  0)) 

{ 

if  (TRACE  Sc&  DISPLAYSCREEN)  printf  ("[LOOPFILEBACKUP]  "); 

LOOPFILEBACKUP  =  TRUE; 

) 

else  if  ((strcmp  (argv[i),  "ENTERCONTROLCONSTANTS" )  ==  0)  II 
(strcmp  (argv[i],  "ENTER-CONTROL-CONSTANTS")  ==  0)) 

{ 

if  (TRACE  &&  DISPLAYSCREEN)  printf  ("[ENTERCONTROLCONSTANTS] 

ENTERCONTROLCONSTANTS  =  TRUE; 

} 

else  if  ((strcmp  (argv[i],  "SHOWCONTROLCONSTANTS" )  ==  0)  II 
(strcmp  (argv[i],  "SHOW-CONTROL-CONSTANTS")  ==0)) 

{ 

if  (DISPLAYSCREEN)  printf  ("[SHOWCONTROLCONSTANTS]  "); 

SHOWCONTROLCONSTANTS  =  TRUE; 

} 

else  if  ({strcmp  (argv[i],  "CONTROLCONSTANTSINPUTFILE" )  ==  0)  I! 

(Strcmp  (argvti],  "CONTROL-CONSTANTS-INPUT-FILE")  ==  0)  II 

(strcmp  (argv[i],  "CONTROL-CONSTANTS-FILE" )  ==  0)  II 

(Strcmp  (argv[i],  "CONTROLCONSTANTSFILE" )  ==  0)) 


} 

else 


{ 


LOADCONTROLCONSTANTS  =  TRUE; 
if  (DISPLAYSCREEN) 

{ 

printf  ("[%s  ",  argv[i]); 

printf  (CONTROLCONSTANTSINPUTNAME) ; 

printf  ("]\n"); 


( (strcmp 

(argv[i] , 

''TACTICAL") 

==  0) 

(strcmp 

(argv[i] , 

"TACTICAL-HOST" } 

===  0) 

(strcmp 

(argv[i] , 

"TACTICALHOST" ) 

==  0) 

(strcmp 

(argv[i] , 

"STRATEGIC") 

==  0) 

(strcmp 

(argv[i] , 

"STRATEGICHOST") 

==  0) 

(strcmp 

(argv[i] , 

"STRATEGIC-HOST") 

'  ==  0)] 

1 

TACTICAL  =  TRUE; 
i  +  +  ; 

if  (i  >=  argc)  print_help  =  TRUE; 
else 


( 


} 

else 

{ 


} 


KEYBOARD INPUT  =  FALSE; 

sscanf  (argv[i],  "%s",  tactical_remote_host_name) 
if  (TRACE  &&  DISPLAYSCREEN) 

printf ("[%s  %s  (set  KEYBOARD-OFF)]", 

argv[i-l] ,  tactical_remote_host_name) ; 


if  ((strcmp  (argv[i] ,  "NO-TACTICAL")  ==  0)  II 
(strcmp  (argv[i] ,  "TACTICAL-OFF")  ==  0)) 

if  (DISPLAYSCREEN)  printf  ("[%s]\n",  argv[i]); 
TACTICAL  =  FALSE; 


} 

else  if  ((strcmp  (argv[i],  "SONARTRACE")  ==  0)  II 

(strcmp  (argv[i],  "SONAR-TRACE")  ==  0)  II 

(strcmp  (argv[i],  "SONAR-TRACE-ON")  ==  0)) 


if  (TRACE  &&  DISPLAYSCREEN)  printf  ("[SONARTRACE]  "); 
SONARTRACE  =  TRUE; 
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else  if  ((strcmp  (argv[i] ,  "SONARTRACEOFF")  ==  0)  II 
(strcmp  (argv[i],  "SONAR-TRACE-OFF" )  ==  0)) 

{ 

if  (TRACE  &&  DISPLAYSCREEN)  printf  ( " [SONARTRACEOFF]  " ) ; 
SONARTRACE  =  FALSE; 

} 

else  if  ((strcmp  (argv[i],  "SONARINSTALLED")  ==  0)  II 
(strcmp  (argv[i],  "SONAR- INSTALLED")  ==  0)) 

{ 

if  (TRACE  &&  DISPLAYSCREEN)  printf  (" [SONAR- INSTALLED]  "); 
SONARINSTALLED  =  TRUE; 

} 

else  if  ((strcmp  (argv[i] ,  "NOSONARINSTALLED" )  ==  0)  M 

(strcmp  (argv[i],  "NO-SONAR- INSTALLED")  ==  0)) 

{ 

if  (TRACE  &&  DISPLAYSCREEN)  printf  (" [NO-SONAR- INSTALLED]  ") ; 
SONARINSTALLED  =  FALSE; 

} 

else  if  ((strcmp  (argv[i] ,  "PARALLELPORTTRACE" )  ==  0)  II 
(strcmp  (argv[i],  " PARALLEL- PORT-TRACE" )  ==  0)) 

{ 

if  (TRACE  &&  DISPLAYSCREEN)  printf  ("[PARALLELPORTTRACE]  "); 
PARALLELPORTTRACE  =  TRUE; 

} 

else  if  ((strcmp  (argv[i],  "SILENT")  ==  0)  II 
(strcmp  (argv[i],  "SILENCE")  ==  0)) 

{ 

if  (TRACE  &&  DISPLAYSCREEN)  printf  ("[SILENT]  "); 

/*  send  to  virtual  world  after  socJcet  is  open  */ 

strcpy  (buffer,  "SILENT");  /*  copy  current  command  to  buffer  */ 

} 

else  if  ((strcmp  (argv[i],  "TIMESTEP")  ==  0)  M 
(strcmp  (argv(i],  "TIME-STEP")  ==  0)) 

{ 

i  +  + ; 

if  (i  >=  argc)  print_help  =  TRUE; 
else 
{ 

#ifndef  os9 

sscanf  (argv[i],  &TIMESTEP)  ; 

#else 

sscanf  (argv[i],  &TIMESTEP) ; 

#endif 

if  (TRACE  ScSc  DISPLAYSCREEN) 

printf ("[TIMESTEP  %3.21f]",  TIMESTEP); 
if  (TIMESTEP  >0.0)  dt  =  TIMESTEP; 
else  if  (TRACE  &&  DISPLAYSCREEN) 

printf ("  illegal  TIMESTEP  value,  ignored."); 
if  (TRACE  ScSc  DISPLAYSCREEN)  printf  ("  [dt  =  %3.21f]",  dt)  ; 

} 

} 

else  if  {(strcmp  (argv[i],  "VIRTUALHOST" )  ==  0)  M 

(strcmp  (argv[i],  "VIRTUAL-HOST")  ==  0)  M 
(strcmp  (argvfij,  "VIRTUAL")  ==  0 )  II 

(strcmp  (argviij,  "REMOTE")  ==0)  || 

(strcmp  (argvfi],  "REMOTEHOST" )  ==  0 )  li 

(strcmp  (argv[i],  "REMOTE-HOST")  ==  0)  M 

(strcmp  (argviij,  "DYNAMIC")  ==  0)  M 

(strcmp  (argv[i],  "DYNAMICS")  ==  0)) 

{ 

i  +  +  ; 

if  (i  >=  argc)  print_help  =  TRUE; 
else 
{ 

sscanf  {argv[i] ,  "%s",  virtual_world_remote_host_name)  ; 
if  (TRACE  &&  DISPLAYSCREEN) 

printf (" [VIRTUAL-HOST  %s]", 
virtual_world_remote__host_name)  ; 
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} 

) 

else  if  ( (strcmp  {argv[i],  "TELEMETRY")  ==  0)  II 

(strcmp  (argv[i],  "TELEMETRYFILE" )  ==  0)  II 

(strcmp  (argv[i],  " TELEMETRY- FILE " )  ==  0)) 


1  +  +  ; 

if  (i  >=  argc)  print_help  =  TRUE; 
else 


) 

else 


} 


sscanf  (argv(i],  "%s",  TELEMETRYFILENAME) ; 
if  (teleinetry_file  =  fopen  (TELEMETRYFILENAME, "r" ) ) 

if  (TRUE  &&  DISPLAYSCREEN) 

printf ("[TELEMETRY-FILE  %s]", 

virtual_world_remote_host_naine )  ; 

REPLAY  =  TRUE; 

) 

else  printf (" [Unable  to  open  telemetry  file:  %s]\n", 
TELEMETRYFILENAME) ; 


} 

else 


if  ((strcmp  (argv[i],  "REALTIME")  ==  0)  II 
(strcmp  (argv(i],  "REAL-TIME")  ==  0)) 

if  (TRACE  &&  DISPLAYSCREEN)  printf  ("[REALTIME]  ") 
REALTIME  =  TRUE; 


} 

else 


if 

( (strcmp 

(argv[i] 

,  "NOREALTIME") 

=  =  0) 

(strcmp 

(argv[i] , 

"NO-REALTIME") 

==  0) 

(strcmp 

(argv[i] , 

"NO-REAL-TIME") 

==  0) 

(strcmp 

(argv[i] , 

"NOWAIT" ) 

==  0) 

(strcmp 

(argv[i] , 

"NO-WAIT") 

==  0) 

(strcmp 

(argv[i] , 

"NO PAUSE" ) 

==  0) 

(strcmp 

(argv[i] , 

"NO-PAUSE") 

==  0)) 

if  (TRACE  &&  DISPLAYSCREEN)  printf  (" 
REALTIME  =  FALSE; 

[NOWAIT 

if 

(  (strcmp 

(argv[i] , 

"EMAIL") 

0)  i  1 

(strcmp 

(argv(i] , 

"EMAIL-ON")  == 

0)  1  1 

(strcmp 

(argv[i] , 

"E-MAIL") 

0)  I  1 

(strcmp 

(argv[i] , 

"E-MAIL-ON")  == 

0)  i  1 

(strcmp 

(argv[i] , 

"EMAILON") 

0)) 

} 

else 


if  (TRACE  &&  DISPLAYSCREEN)  printf  ("\n[EMAIL  ON]  "); 
EMAIL  =  TRUE; 


( (strcmp 

(argv[i] , 

"EMAILOFF") 

== 

0) 

(strcmp 

(argv[i] , 

"E-MAILOFF") 

=  zz 

0) 

(strcmp 

(argv[i] , 

"EMAIL-OFF") 

== 

0) 

(strcmp 

(argv(i] , 

"E-MAIL-OFF") 

0) 

(strcmp 

(argv(i] , 

"NO-E-MAIL") 

=  = 

0) 

(strcmp 

(argv[i] , 

"NO-EMAIL") 

=  = 

0) 

(strcmp 

(argv[i] , 

"NO-E-MAIL")' 

=  = 

0) 

(strcmp 

(argv[i] , 

"NOEMAIL") 

=  = 

0) 

} 

else 


} 

else 


if  (TRACE  &&  DISPLAYSCREEN)  printf  ("[NO  EMAIL]"); 
EMAIL  =  FALSE; 


if  ((strcmp  (argv[i], 
(strcmp  (argv[i), 

LOCATIONLAB  =  TRUE; 


"LOCATIONLAB" )  ==  0) 
"LOCATION-LAB")  ==  0)) 


if  ((Strcmp  (argv[i],  "TETHER")  ==  0)  II 
(strcmp  (argvli],  "TETHERED")  ==  0)) 
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#ifndef 
#else 
#endi f 


#ifndef 

#else 


{ 

LOCATIONLAB  =  FALSE; 
DISPLAYSCREEN  =  TRUE; 
REALTIME  =  TRUE; 

} 


else 

if  ( (strcmp 

(argv[i],  "UNTETHER") 

=  = 

0) 

(strcmp 

(argv [ i ] ,  "UNTETHERED" ) 

0) 

(strcmp 

(argv[i],  "NOTETHER") 

=  = 

0) 

{ 

(strcmp 

(argv[i],  "NO-TETHER") 

== 

0)) 

LOCATIONLAB 

=  FALSE; 

DISPLAYSCREEN  =  FALSE; 

) 

else 

REALTIME 

=  TRUE; 

if  { (strcmp 

(argv[i],  "TEXT") 

0) 

1 1 

{ 

} 

else 

(strcmp 

(argvfij,  "TEXT-ON")  == 

0)  ) 

DISPLAYSCREEN  =  TRUE; 

if  (  (strcmp 

(argv(i),  "NOTEXT")  == 

0) 

1 1 

{ 

} 

else 

(strcmp 

(argv[i],  "NO-TEXT")  == 

0)) 

DISPLAYSCREEN  =  FALSE; 

if  { (strcmp 

(argv[i),  "GYROERROR") 

== 

0) 

(strcmp 

(argv[i],  "GYRO-ERROR") 

=  = 

0) 

(strcmp 

(argv[i),  "GYRO  ERROR") 

== 

0)) 

{ 

i  +  + ; 

if  (i  >=  argc) 

{ 

print_help  =  TRUE; 
if  (DISPLAYSCREEN) 

printf  warning:  invalid  GYRO-ERROR  command. \n" ) ; 

} 

else 

{ 

os9 

sscanf  (argv[i},  "%lf",  gyro_error) ; 


sscanf  (argv[i],  ,  gyro_error) ; 


} 

else 


{ 


os9 


if  (TRACE  &&  DISPLAYSCREEN) 

printf ("[%s  %5.21f]",  argv[i-l],  gyro_error) ; 

} 


if  ((strcmp  (argv[i],  "DEPTH-CELL-BIAS")  ==  0)  I 

(strcmp  (argv[i],  "DEPTHCELLBIAS" )  ==0)  I 

(strcmp  (argv[i],  "DEPTH-CELL- ERROR" )  ==  0)  I 

(strcmp  (argv[i],  "DEPTHCELLERROR" )  ==  0)  I 

(strcmp  (argv(i),  "DEPTH-BIAS")  ==  0)  I 

(strcmp  (argv[i],  "DEPTHBIAS")  ==  0)  I 

(strcmp  (argv[i],  " DEPTH- ERROR " )  ==  0)  I 

(strcmp  (argv[i],  "DEPTHERROR" )  ==  0)) 


i++ ; 

if  (i  >=  argc) 

{ 

print_help  =  TRUE; 
if  (DISPLAYSCREEN) 

printf  ("  warning:  invalid  DEPTH-CELL-BIAS  command. \n") 

} 

else 

{ 


sscanf  (argv[i],  "%lf",  &  depth_cell_bias) ; 


sscanf  (argv[i],  "%F",  &  depth_cell_bias) ; 
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iendif 


if  (TRACE  &&  DISPLAY SCREEN) 

printf("[%s  %5.21f]",  argv[i-l],  depth„cell_bias) ; 

} 

else  if  ( (strcmp  (argv[i],  "BENCH")  ==  0)  II 

(strcmp  (argv[i],  "BENCH-TEST")  ==0)  il 

(strcmp  (argv[i],  "BENCHTEST")  ==  0)) 


DISPLAYSCREEN  =  TRUE; 
LOCATIONLAB  =  FALSE; 
KEYBOARDINPUT  =  TRUE; 
DIVETRACKER  =  FALSE; 
EMAIL  =  FALSE; 

REALTIME  =  TRUE; 

BENCHTEST  =  TRUE; 

} 


else 

{ 


if  ( (strcmp  (argv[i] 
(strcmp  (argv[i] 


NOSCRIPT  =  TRUE; 

} 

else  if  ((strcmp  (argv[i], 
(strcmp  (argv[i], 
(strcmp  (argvfi], 
(strcmp  (argvii], 

{ 

i  +  + ; 

if  (i  >=  argc) 

{ 


"NOSCRIPT")  ==  0)  !l 
"NO- SCRIPT")  ==  0)) 


"MISSION")  ==0)  II 
"SCRIPT")  ==0)  II 
"FILE")  ==  0)  M 
"FILENAME")  ==  0 )  ) 


print_help  =  TRUE; 
if  (DISPLAYSCREEN) 

printf  ("  warning;  invalid  %s  command. \n". 


else 


argv  [  i  ]' )  ; 


#ifndef  os9 

#else 

#endif 


strcpy  (new_f ilename,  argv[i] ) ; 
if  (DISPLAYSCREEN) 

printf  ("\n[%s  %s]\n",  argv[i-l],  new_f ilename) 

sprintf  (backupcommand,  "cp  %s  %s",  new_f ilename, 
AUVSCRIPTFILENAME) ; 

sprintf  (backupcommand,  "copy  %s  %s",  new_f ilename, 
AUVSCRIPTFILENAME) ;  . 


#ifndef  os9 


#endif 


} 

else  if 

{ 


if  .(DISPLAYSCREEN) 

printf  ("%s\n",  backupcommand)  ; 
system_result  =  system  (  backupcommand) ; 

if  (TRACE  &&  DISPLAYSCREEN) 

printf  ("system_result  =  %d\n",  system_result ) ; 
if  (system_result  ==  512) 

{ 

printf  ("system_result  =  %d\n",  system__result ) ; 
printf  ("Mission  not  found.  Exit.Xn"); 
exit  (-1) ; 

} 

auvscriptfile  ==  NULL;  /*  force  re-read  */ 


((strcmp  (argv[i],  "SCANWIDTH")  ==  0)  I  I 
(strcmp  (argv[i],  "SCAN-WIDTH")  ==  0)) 


i  +  +  ; 

if  (i  >=  argc)  print_help  =  TRUE; 
else 
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{ 

tifndef  os9 

sscanf  (argv[i],  &SCANWIDTH) ; 

#else 

sscanf  (argvfi],  &SCANWIDTH) ; 

#endif 

if  (DISPLAYSCREEN)  printf ( " [ SCANWIDTH  %3.21f]'S  SCANWIDTH) ; 
if  ({SCANWIDTH  <=  0.0)  &&  DISPLAYSCREEN) 

{ 

printf  ("  illegal  SC7\NWIDTH  value,  ignored."); 
printf("  [SCANWIDTH  =  %3.21f]",  SCANWIDTH); 

} 

} 

} 

else  print_help  =  TRUE; /* invalid  command  line  entry  parameter  found  */ 

}  /*  end  for  loop  through  command  line  parameters  */ 

if  {print_help)  /*  print  help  string  ************************************/ 

{ 

printf ("\nUnreckogni zed  keyword.  Usage:  execution  \n"); 
print_valid_keywords  ()  ; 
exit  (-1); 

} 

if  (TRACE  &&  DISPLAYSCREEN) 

printf  {"\n[parse_command_line_flags  complete] \n" ) ; 

return; 

}  /*  end  parse_command_line_f lags  ()  */ 

/***************************************************************************/ 

int  parse_mission_script_commands  ()  /*  get  data  from  file  at  program  start  */ 
^  /*  mission. script . HELP  =>  descriptions  */ 

/*  command_buffer  is  the  string  to  be  parsed  */ 

int  index,  read_another_line,  parameters_read; 
doubl e  parameterl , parameter2 , parameter3 , parameter4 , parameters , parameters ; 

char  parameter_string  [60]; 
int  return_value; 

read_anot hardline  =  TRUE; 

/*  If  Shutdown  in  Progress,  Ignore  Commands  and  Go  to  Shutdown  Script  */ 
if  (HALTSCRIPT) 

{ 

return  (FALSE); 

} 

/*  do  not  skip  to  next  command  in  KEYBOARD  or  script  mode  until  ready  */ 
if  ((t  <  time_next_command)  &&  (TACTICAL  ==  FALSE) 

&&  (TACTICALPARSE  ==  FALSE)) 

{ 

if  (TRACE  &&  DISPLAYSCREEN) 

{ 

printf  ("\n[skip  parse_mission_script_commands  ()  until  "); 
printf  ("t  >  time_next_coinmand]  \n")  ; 

} 

return  (FALSE) ; 

} 

if  (TRACE  &&  DISPLAYSCREEN) 

printf  ("\n[start  parse_mission_script__commands  ()]\n"); 
if  (  (GPSFIXINPROGRESS)  &Sc  (t  >=  time_postgps_dive)  ) 
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{ 

if  (TACTICAL)  /*  execution  tell  tactical  gpg-fix  done  */ 

{ 

if  (TRACE  &&  DISPLAYSCREEN) 
printf 

("\n[send__buffer_to_tactical_socket  (STABLE  GPS  TIMEOUT)]"); 
strcpy  (buffer,  "STABLE  GPS  TIMEOUT"); 
send__buf fer_to_tactical_socket  ();  /*  message  */ 

} 

GPSFIXINPROGRESS  =  FALSE; 
read_another__line  =  FALSE; 

} 

if  ( (GPSFIXINPROGRESS)  &&  (t  >=  time_^ps_complete)  && 

(t  <  time_postgps_dive) ) 

{ 

z_coinmand  =  previous__z_coirmiand; 

time  JOS  tgps_dive  =  t  +  30.0;  /*  head  back  to  ordered  depth  */ 

timejextjoramand  =  time__postgps_dive; 
timejps  ^complete  =  timejostgps_dive  +  1.0; 
read_another_linG  =  FALSE; 

if  (DISPLAYSCREEN)  printf  ("\n[GPS-FIX  complete ,] \n" ) ; 

} 

/*  Only  look  at  auvscriptf ile  if  we  are  in  script  file  execution  mode  */ 

if  (  (TACTICAL PARSE)  /*  tactical  level  internal  use  */ 

i I  (KEYBOARD INPUT)  /*  execution  level  */ 

II  (TACTICAL))  /*  execution  level  getting  tactical  comms*/ 

{ 

/*  no  auvscriptf ile  setup  required  in  these  modes  */ 

} 

else  if  (  (auvscriptfile  ==  NULL)/*  auvscriptf ile  not  yet  opened  */ 

I  I  feof  (auvscriptfile)  /*  auvscriptfile  end-of-file,  repeat  */ 

I  I  auvscriptfilequit)  /*  flag  for  all  done  */ 

{ 

if  (DISPLAYSCREEN) 

{ 

printf  ("\n [opening  a  copy  of  the  auvscriptfile  %s]\n", 
AUVSCRIPTFILENAME) ; 
f flush  (stdout); 

} 

#ifndef  os9 

sprintf  (backupcommand,  "rm  %s. backup",  AUVSCRIPTFILENAME); 

if  (DISPLAYSCREEN)  printf  ("%s\n",  backupcommand) ; 
system  (backupcommand) ; 

sprintf  (backupcommand,  "cp  %s  %s. backup",  AUVSCRIPTFILENAME, 

AUVSCRIPTFILENAME) ; 

if  (DISPLAYSCREEN)  printf  ("%s\n",  backupcommand); 
system  (backupcommand) ; 

#else 

/*  OS-9  */ 

sprintf  (backupcommand,  "del  %s. backup",  AUVSCRIPTFILENAME); 

if  (DISPLAYSCREEN)  printf  ("%s\n",  backupcommand); 
system  (backupcommand) ; 

sprintf  (backupcommand,  "copy  %s  %s. backup",  AUVSCRIPTFILENAME, 

AUVSCRIPTFILENAME)  ; 

if  (DISPLAYSCREEN)  printf  ("%s\n",  backupcommand); 
system  (backupcommand) ; 

#endif 


sprintf  (backupcommand,  "%s. backup",  AUVSCRIPTFILENAME) ; 
auvscriptfile  =  fopen  (backupcommand, "r" ) ;  /*  input  file  */ 

if  (auvscriptfile  ==  NULL) 

{ 

printf  ("AUV  execution:  script  file  %s\n",  AUVSCRIPTFILENAME); 
printf 

{"  (or  backup  copy  %s. backup)  not  found. \n". 
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backup  command)  ; 

printf  {"  Ensure  you  are  in  the  right  directory : \n" ) ; 

printf  ("  auvsiml>  chd  /hO/execution  or\n"); 

printf  ("  unix>  cd  -brutzman/executionXn" ) ; 

printf 

{"  Otherwise  ensure  you  have  a  %s  file.Xn", 

AUVSCRIPTFILENAME) ; 

printf  ("Exit.Xn"); 
exit  (-1); 

} 

auvscriptf ilequit  =  FALSE; 

} 

else  if  (TRACE  ScSc  DISPLAYSCREEN) 

printf  ("Xn [auvscriptf ile  checks  out  as  ready ...] Xn" ) ; 

if  ( (TACTICALPARSE  ==  FALSE)  &&  {NOSCRIPT  ==  FALSE) ) 

{ 

/*  open  auvordersfile  - 

sprintf  (buffer,  AUVORDERSFILENAME)  ; 

if  (auvordersfile  ==  NULL) 

{ 

auvordersfile  =  fopen  (buf fer, "w" ) ;  /*  output  file  */ 

if  (TRACE  ScSc  DISPLAYSCREEN) 

printf  ("Xn[ auvordersfile  =  %x,  opened  successfully] Xn", 
auvordersfile) ; 

fprintf  (auvordersfile,  "XnXn"); 
fprintf  (auvordersfile, 

"#  NPS  AUV  file  %s:  commanded  propulsion  orders  versus  timeXn", 
AUVORDERSFILENAME) ; 
fprintf  (auvordersfile,  "#\n"); 

fprintf  (auvordersfile,  timestep:  %4.2f  secondsXn",  dt); 

fprintf  (auvordersfile,  "#Xn"); 
fprintf  { au vorders  file. 


"#  time  heading  North  East  Depth 
lateral  Xn"); 

rpm 

rpm 

stern 

stern 

vertical 

fprintf  (auvordersfile. 

"  #  X  y  z 

port 

stbd 

plane 

rudder 

thrusters 

thrustersXn" ) ; 

fprintf  (auvordersfile, 

"#  bow/s  tern  bow/ 

sternXn" )  ; 

fprintf  (auvordersfile,  "Xn"); 

} 

else  if  (TRACE  &&  DISPLAYSCREEN) 

printf  ("auvordersfile  (%s)  =  %xXn",  AUVORDERSFILENAME, 
auvordersfile) ; 

if  ((auvordersfile  ==  NULL)  &&  (NOSCRIPT  ==  FALSE)) 

{ 

printf  ("AUV  execution:  %s  file  open  unsuccessful . Xn" ,  buffer); 
printf  ("  Error. Xn"); 

printf  ("Exit.Xn"); 
exit  (-1); 

} 


while  (read_another_line)  /*  *********  Parse  loop  ************  */ 

{ 

parameterl  =0.0; 
parameter2  =  0.0; 
parameters  =0.0; 

/*  Four-way  switch:  tactical  level  parses  commands  internally,  or  */ 
y*  execution  level  parses  commands  from  */ 
/*  keyboard  I  tactical  ood  I  mission  .script  file  */ 
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/*  each  option  gets  the  next  order  and  puts  it  in  coininand^buf fer  */ 

if  (TACTICALPARSE)  /*  tactical  level  internal  use  .*/ 

{ 

/*  coinmand_buf fer  is  already  sent  and  ready  */ 
read_another_line  =  FALSE; 

} 

else  if  (KEYBOARDINPUT)  /*  this  blocks!  */ 

{ 

strcpy  (buffer,  "Enter  command" ) ; 

/*  send_buf fer_to__virtual_world_socket  {);  /*  buffer  msg  sent  */ 

printf  {"\n%s  ***  HERE  ***:  ",  buffer); 

strcpy  (command_buffer, 
gets  (command_buf fer) ; 

} 

else  if  (TACTICAL)  /*  execution  level  getting  tactical  comms  */ 

^  /*  get  command_buffer  string  from  tactical  level,  nonblocking  */ 

/*  initialize  sonar  data  in  case  it  is  generated  by  tactical*/ 
if  (TRACE  £cSc  DISPLAYSCREEN) 

printf  ("\n  RECEIVE  TACTICAL  COMMAND  ***  HERE  ***\n"); 
strcpy  (command_buf f er ,  "")’; 
get_string_f rom_tactical_socket  (} ; 
if  (strlen  (command^buf fer)  ==0}  /*  no  command  was  received  */ 

{ 

time__next_command  =  t  +  dt;  /*  same  as  STEP  command  */ 
tead_another_line  =  FALSE;  /*  (prevent  blocking)  */ 
if  (TRACE  DISPLAYSCREEN) 

printf ("no  tactical  command  received,  STEP  &  recheckXn"); 
break; 

} 

} 

else  /*  get  command_buf fer  string  from  auvscriptfile  */ 

strcpy  {command_buf fer,  ""); 

fgets  (command^buffer,  120,  auvscriptfile); 

if  (feof  (auvscriptfile)) 

{ 

if  (DISPLAYSCREEN) 

{ 

printf  ("\n[EOF  condition:  (") ; 

printf  ("%s  copy)  %s. backup,  file  closed] \n", 

AUVSCRIPTFILENAME,  AUVSCRIPTFILENAME) ; 

fclose  (auvscriptfile) ; 
auvscriptfilequit  =  TRUE; 
read_another_line  =  FALSE; 
end_t  e  s  t  =  TRUE ; 

strcpy  (command^buf fer,  ""); 
break ; 

} 

} 


/*  parse  the  command,  if  any -  */ 

if  { (int) (strlen  (command_buf fer)  <=  120)  &&  TRACE  &&  DISPLAYSCREEN) 

{ 

printf  ("strlen  {command_buf fer)  =  %d" ,  strlen  (command_buffer) ) ; 
printf  {">»%s«<",  command_buf fer)  ; 


parameters_read  =  sscanf  (command_buf fer,  "%s",  keyword); 
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#ifndGf 


#elsG 


#endi f 


AUV  */ 


if 

{ 

} 


{TRACE  &&  DISPLAYSCREEN) 

print f  ( "parameters^read  =  %d,  keyword  =  %s" , 
parameters^read,  keyword) ; 


for  (index=0;  index<= (int) strlen  (keyword);  index++)/*  set  uppercase  */ 
keyword  [index]  =  toupper  (keyword  [index]); 

audible_cominand  =  TRUE; 


if  (TRACE  &&  DISPLAYSCREEN) 

{ 

printf  uppercase  keyword  =  %s\n",  keyword); 

} 


if  ( (parameters^read  !=  1)  II 

(strlen  (keyword)  ==  0)  II 

(strlen  (command_buf fer)  ==  0)  11 

(coininand^buffer  [0]  ==  '\n'))  /*  blank  line  */ 

{ 

audible_command  =  FALSE; 
read__another_line  =  TRUE; 
if  (DISPLAYSCREEN)  printf  ("\n"); 

} 

else  if  (  keyword  [0]  ==  /*  comment  */ 

{ 

if  (DISPLAYSCREEN)  printf  {"%s",  command_buff er) ; 

command_buf fer  [0]  =  '  '; 

} 

else  if  {((keyword  [0]  ==  '/')  &&  (keyword  [1]  ==  '/'))  II 

({keyword  [0]  ==  V')  &&  (keyword  [1]  ==  '*')))  /*  comment  */ 

{ 


if  (DISPLAYSCREEN) 
command_buf f er  [0] 
command_buf f er  [1] 

} 

else  if  { (strcmp  (keyword, 
(strcmp  (keyword, 
(strcmp  (keyword, 
(strcmp  (keyword, 

{ 


printf  ("%s",  command_buffer) ; 
/  /  . 


"HELP")  ==  0)  II 
"?")  ==  0)  II 
"-?-)  ==  0)  II 
"/?")  ==  0)) 


if  (DISPLAYSCREEN)  printf  ("\n[HELP]  "); 
print_valid_keywords  ( ) ; 


else  if 

{ 

os9 


( (strcmp 
(strcmp 
(strcmp 


(keyword, 

(keyword, 

(keyword. 


"SONAR_725") 

"SONAR725") 

"SONAR-725") 


0)  I  I 
0)  I  I 
0)  ) 


parameters_read  =  sscanf  (command__buf fer,  "%s%lf%lf%lf", 

keyword,  &parameterl,  Scparameter2, 
&parameter3 ) ; 


parameters_read  =  sscanf  (command_buf fer,  "%s%F%F%F", 

keyword,  &parameterl,  Scparameter2, 
^parameters ) ; 

if  (DISPLAYSCREEN  &&  LOCATIONLAB) 

printf  ("\n[%s  %6.2f  %6.2f  %6.2f]\n",  keyword,  parameterl, 

parameter2,  parameters); 

AUV_ST725_bearing  =  normalize  (parameterl);  /*  controlled  by 
if  (LOCATIONLAB  ==  FALSE) 

{  /*only  virtual  world  provides  sonar  values  when  out  of  water  */ 
AUV_ST7 2  Strange  =  paramGter2; 

AUV_ST72  5_strength  =  parameters,* 
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} 

else  if  ( (strcinp  (keyword,  "SONAR_1000")  ==  0)  II 
(strcmp  (keyword,  "SONARIOOO")  ==  0)  II 
(strcmp  (keyword,  "SONAR~1000" )  ==  0)) 

{ 

#ifndef  os9 


#else 


parani€ters_read  =  sscanf  (command_buffer,  "%s%lf", 

keyword,  &pararaeterl)  ; 


#endif 


} 


parameter spread  =  sscanf  (command_buf fer,  "%s%F", 

keyword,  &parameterl) ; 


if  (DISPLAYSCREEN  &&  LOCATIONLAB) 

printf  ("\n[%s  %6.2f]\n",  keyword,  parameter!); 

SONARS CANMODE  =5;  /*  manual  control  */ 

Stl000_command  =  SONARHEADINGSTEP  * 

{ (int)  (normalize (parameter!)  /  SONARHEADINGSTEP) ) ; 


else  if  ((strcmp  (keyword,  "POSITION")  ==  0)  li 

(strcmp  (keyword,  "LOCATION")  0)  II 

(strcmp  (keyword,  "FIX")  ==  0)  ) 

/*  note  this  command  must  be  sent  to  virtual  world  (AUVsocket.C  tests)  */ 

{ 

tifndef  os9 


#else 


#endif 


parameters_read  =  sscanf  (command_buf f er ,  "%s%lf %1 f %lf " , 

keyword,  &  parameter!, 

&  parameter2,  &  parameter3); 

parameters_read  =  sscanf  (command_buf f er ,  "%s%F%F%F", 

keyword,  &  parameter!, 

&  parameter2,  &  parameters); 


if  (parameters__read .  ==  4) 

{ 

X  =  parameter!; 
y  =  parameters  ; 

z  =  parameters,*  /*  note  depth  cell  will  likely  update  z  */ 
/*  skip  line  in  telemetry  file  to  break  point-to-point  lines  */ 
if  ( (TACTICALPARSE)  M  (TACTICAL  ==  FALSE) ) 
fprintf  (auvdataf ile,  "\n"); 


if  (DISPLAYSCREEN) 

printf  ("\n[%s  %6.2f  %6,2f  %6.2f]\n",  keyword, 

parameter!,  parameters,  parameters); 
if  (TRACE  &&  DISPLAYSCREEN) 

printf  ("\nsending  fix  to  virtual  world:  [%s]\n", 

buffer) ; 

strcpy  (buffer,  command_buf fer) ;  /*  copy  command  to  buffer*/ 
send_buf fer__to_virtual_world_socket  (),*  /*  send  to  vw  */ 

} 

else  if  (parameters_read  ==  S) 

{ 

X  =  parameter!; 
y  =  parameters,* 

/*  skip  line  in  telemetry  file  to  break  point-to-point  lines  */ 
if  ((TACTICALPARSE)  I  I  (TACTICAL  ==  FALSE) ) 
fprintf  (auvdatafile,  "\n"); 


if  (DISPLAYSCREEN) 

printf  ("\n[%s  %6.2f  %6.2f]\n",  '  keyword,  parameter!, 

parameters ) ; 

if  (TRACE  ScSc  DISPLAYSCREEN) 

printf  ("\nsending  fix  to  virtual  world:  [%s]\n",  buffer); 
strcpy  (buffer,  command_buffer) ;  /*  copy  command  to  buffer*/ 
send_buf fer_to_virtual_world__socket  ();  /*  send  to  vw  */ 


else  if  (DISPLAYSCREEN) 
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} 

else 

{ 

#ifndef  os9 

#else 

#endif 


printf  {"  warning:  invalid  x/y/z  fix  position,  ignoredXn") 

if  ({strcmp  (keyword,  "HOVER'')  ==  0)  M 
(strcmp  (keyword,  "HOVER-ON")  ==  0) ) 


parameters_read  =  sscanf  (cominand_buf  fer,  "%s%lf%lf  %lf %lf  %lf  "  , 

keyword,  &  parameterl, 
Sc  parainecer2 ,  Sc  parameters, 
Sc  parameter4.  Sc  parameters) 

parameters__read  =  sscanf  (command_buf f er,  "%s%F%F%F%F%F" , 

keyword,  Sc  parameterl. 
Sc  parameter2.  Sc  parameters, 
Sc  parameter4.  Sc  parameters) 

if  (parameters_read  ==  6) 

{ 

if  (DISPLAYSCREEN) 

printf  ("\n[%s  %6.2f  %6.2f  %6.2f  %6.2f  %6.2f]\n", 

keyword,  parameterl , 
parameter2,  parameters, 
parameter4,  parameters) 

HOVERCONTROL  =  TRUE; 

REPORTSTABLE  =  TRUE; 

WAYPOINTCONTROL  =  FALSE; 

ROTATECONTROL  =  FALS  E ; 

LATERALCONTROL  =  FALS  E ; 

THRUS  TERCONTROL  =  TRUE ; 

TARGETCONTROL  =  FALS  E ; 

RECOVER YCONTROL  =  FALSE; 

INTEGRALDEPTHCONTROL  =  FALSE; 

time_int_control_on  =  t  +  10.0;  /*give  PD  10  seconds  */ 
SONARSCANMODE  =1;  /*  Forward  Scan  */ 

DEADSTICKRUDDER  =  TRUE; 

DEATH_SPIRAL_RESET  =  TRUE; 

rudder_command  =  0.0; 

x_command  =  parameterl; 

y_command  =  parameter2; 

z_command  =  parameters,- 

psi_coinmand  =  normalize  (parameter4); 

psi_command_hover  =  psi_command; 

standof  f_distance  =  parameters,- 

} 

else  if  (parameters_read  ==  S) 

{ 

if  (DISPLAYSCREEN) 

printf  {"\n[%s  %6.2f  %6.2f  %6.2f  %6.2f]\n", 

keyword,  parameterl , 
parameter2 ,  parameters , 
parameter4); 

HOVERCONTROL  =  TRUE; 

REPORTSTABLE  =  TRUE; 

WAYPOINTCONTROL  =  FALSE; 

ROTATECONTROL  =  FALSE; 

LATERALCONTROL  =  FALSE; 

TARGETCONTROL  =  FALSE; 

RECOVER YCONTROL  =  FALSE; 

INTEGRALDEPTHCONTROL  =  FALSE; 

time_int_control_on  =  t  +  10.0;  /*  give  PD  10  seconds  */ 
SONARSCANMODE  =1;  /*  Forward  Scan  */ 

THRUSTERCONTROL  =  TRUE; 

DEADSTICKRUDDER  =  TRUE; 

DEATH_SPIRAL_RESET  =  TRUE; 

rudder_command  =  0.0; 

x^command  =  parameterl; 

y_command  =  parameter2; 

z_command  =  parameters,* 
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psi^command  s=  normalize  {paraineter4)  ; 

psi_coinmand_hover  =  psi_command; 

) 

else  if  (parameters_read  ==  4) 

{ 

if  (DISPLAYSCREEN) 

printf  ("\n[%s  %6.2f  %6.2f  %6.2f]\n", 

keyword,  parameterl , parameter2 ,  parameters ) ; 
HOVERCONTROL  =  TRUE; 

REPORTSTABLE  =  TRUE; 

WAYPOINTCONTROL  =  FALSE; 

ROTATECONTROL  =  FALSE; 

LATERALCONTROL  =  FALSE; 

TARGETCONTROL  =  FALSE; 

RECOVERYCONTROL  =  FALS  E ; 

INTEGRALDEPTHCONTROL  =  FALSE; 

time__int_control_on  =  t  +  10.0;  /*  give  PD  10  seconds  */ 
SONARSCANMODE  =1;  /*  Forward  Scan  */ 

THRUSTERCONTROL  =  TRUE; 

DEADSTICKRUDDER  =  TRUE; 

DEATH_SPIRAL_RESET  =  TRUE; 
rudder^command  =  0.0; 
x__coininand  =  parameterl; 

y__command  =  parameter2 ; 

z_command  =  parameters ; 

psi_command_hover  =  psi_command; 

} 

else  if  (parameters_read  ==  S) 

{ 

if  (DISPLAYSCREEN) 

printf  ("\n[%s  %6.2f  %6.2f]\n",  keyword,  parameterl, 

parameter2 ) ; 

HOVERCONTROL  =  TRUE; 

REPORTSTABLE  =  TRUE; 

WAYPOINTCONTROL  =  FALSE; 

ROTATECONTROL  =  FALS  E ; 

LATERALCONTROL  =  FALSE; 

TARGETCONTROL  =  FALSE; 

RECOVERYCONTROL  =  FALS  E ; 

INTEGRALDEPTHCONTROL  =  FALSE; 

time_int_control_on  =  t  +  10.0;  /*  give  PD  10  seconds  */ 
SONARSCANMODE  =1;  /*  Forward  Scan  */ 

THRUSTERCONTROL  =  TRUE; 

DEADSTICKRUDDER  =  TRUE; 

DEATH_SPIRAL_RESET  =  TRUE; 
rudder_command  =  0.0; 
x_command  =  parameterl; 

y_command  =  parameters,* 

psi_command_hover  =  psi_command; 

} 

else  if  (parameters_read  ==  1) 

{ 

if  (DISPLAYSCREEN)  printf  ("\n(%s]\n",  keyword); 
HOVERCONTROL  =  TRUE; 

REPORTSTABLE  =  TRUE ; 

WAYPOINTCONTROL  =  FALSE; 

ROTATECONTROL  ==  FALS  E  ; 

LATERALCONTROL  =  FALSE; 

TARGETCONTROL  =  FALSE ; 

RECOVERYCONTROL  =  FALS  E ; 

INTEGRALDEPTHCONTROL  =  FALSE; 

time_int_control_on  =  t  +  10.0;  /*  give  PD  10  seconds  */ 
SONARSCANMODE  =1;  /*  Forward  Scan  */ 

THRUSTERCONTROL  =  TRUE; 

DEADSTICKRUDDER  =  TRUE; 

DEATH_SPIRAL_RESET  =  TRUE; 

rudder_command  =  0.0; 

x_coinmand  =  x;  /*  stay,  put!  */ 
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} 

else 


if 

{ 


y_cominand  =  y; 

z_coinmand  =  z; 

psi_cominand_hover  =  psi;  /*  "Meet  Her"  */ 

} 

else 

{ 

if  (DISPLAYSCREEN) 

{ 

printf  {"\n[%sj\n",  keyword); 
printf  ("  warning:  improper  number  of  values,  ignoredXn"); 
} 


if  { (strcmp  {keyword,  "GPS")  ==  0)  II 

(strcmp  (keyword,  "GPSFIX")  ==  0)  M 

(strcmp  (keyword,  "GPS-FIX")  ==  0)  ) 

(TACTICALPARSE  ==  FALSE) 


previous_z_command  =  z_command; 

if  (z^command  >  40.0)  2__command  =  41.0;  /*  deep  test  tank  */ 

else  z_command  =0.0;  /*  rapid  shallow  */ 

GPSFIXINPROGRESS  =  TRUE; 

time_gps_complete  =  t  +  30.0; 

time_postgps_dive  =  t  +  60.0; 

time_next_coinmand  =  tim€_gps__complete; 


/*  fixed  guestimate  of  GPS  fix  interval  */ 

/*  assume  GPS-FIX  behavior  is  properly  controlled  by  tactical  level 

} 


) 

else 


if  (DISPLAYSCREEN)  printf  ( " \n [GPS-FIX] \n" ) ; 


if  ( (GPSFIXINPROGRESS)  && 

((strcmp  (keyword,.  "GPS -COMPLETE"  ) 
(strcmp  (keyword,  "GPS-FIX-COMPLETE" ) 
(strcmp  (keyword,  "GPSCOMPLETE" ) 

(strcmp  (keyword,  "GPSFIXCOMPLETE" ) 


==  0)  II 
==  0)  II 
==  0)  II 
==  0))  ) 


{ 


z_command  =  previous_z_command; 

time_postgps_dive  =  t  +  30.0;  /*  head  back  to  ordered  depth 

time_next_coinmand  =  time_postgps_dive; 
time__gps_complete  =  time_postgps_dive  +  1.0; 
read_another_line  =  FALSE; 

if  (DISPLAYSCREEN)  printf  ("\n [GPS-FIX  complete. ]\n"); 


else  if  ({strcmp  (keyword, 
(strcmp  (keyword. 


{ 

#ifndef  os9 


"TARGET- STATION")  ==  0)  M 
"TARGETSTATION" )  ==  0 ) ) 


*/ 


V 


#else 


#endif 


parameters__read  =  sscanf  (command_buf fer,  "%s%lf%lf %lf%lf %lf " , 

keyword,  &  parameterl,  &  parameter2. 

Sc  parameter3,  &  parameter4, 

&  parameters) ; 

parameters__read  =  sscanf  {command_buff er,  "%s%F%F%F%F%F" , 

keyword,  &  parameterl,  &  parameter2, 

&  parameter3,  &  parameter4. 

Sc  parameters)  ; 


if  (parameters_read  ==  3) 

{ 

if  (DISPLAYSCREEN) 

printf  ("\n[%s  %6.2f  %6.2f]\n". 


TARGETCONTROL 

NEWTARGET 

RECOVERYCONTROL 

TARGETPOINTING 


=  TRUE; 

=  TRUE; 

=  FALSE; 
=  TRUE; 


keyword,  parameterl , 
parameter2 ) ; 
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TARGETEDGETRACK 

HOVERCONTROL 

WAYPOINTCONTROL 

FOLLOWWAYPOINTMODE 

LATERALCONTROL 

ROTATECONTROL 

REPORTSTABLE 

NEWTARGETSTATION 

targe  t_r  ange_c  ommand 

targe  t_bearing__coTnmand 


} 

else  if  (paraineters_read  ==  4) 


FALSER- 

FALSE; 

FALSE; 

FALSE; 

FALSE; 

FALSE; 

TRUE; 

TRUE; 

parameterl ; 

normalize  (paraineter2)  ; 


{ 


if  {DISPLAYSCREEN) 
printf  ("\n[%s 


TARGETCONTROL 

NEWTARGET 

RECOVERYCONTROL 

TARGETPOINTING 

TARGETEDGETRACK 

HOVERCONTROL 

WAYPOINTCONTROL 

FOLLOWWAYPOINTMODE 

LATERALCONTROL 

ROTATECONTROL 

REPORTSTABLE 

NEWTARGETSTATION 

t  arge  t__range_command 

targe t_bearing_command 

psi_c  ommand 


} 

else  if  (parameters_read  ==  5) 


%6.2f  %6.2f  %6.2f]\n"R  keyword, 

parameterl,  parameter2 , 
parameters ) ; 

=  TRUE; 

=  TRUE; 

=  FALSE; 

=  FALSE; 

=  FALSE; 

=  FALSE; 

=  FALSE; 

=  FALSE; 

=  FALSE; 

=  FALSE; 

TRUE; 

TRUE; 

parameterl ; 

normalize  (parameter2) ; 
normalize  (parameters); 


{ 


if  (DISPLAYSCREEN) 
printf  ("\n[%s 


=  4; 


Locate  Target  with  STIOOO  */ 


%6.2f  %6.2f  %6.2f  %6.2f]\n",  keyword, 

parameterl,  parameter2, 
parameters ,  parameter4 ) ; 

=  TRUE; 

=  TRUE; 

FALSE; 

/* 

TRUE  ; 

FALSE; 

FALSE; 

FALSE; 

FALSE; 

FALSE; 

FALSE; 

TRUE; 

TRUE; 

parameters  ; 

normalize  (parameter4); 
parameterl ; 

normalize  (parameters); 


TARGETCONTROL 
NEWTARGET 
RECOVERYCONTROL 
SONARSCANMODE 
TARGETPOINTING 
TARGETEDGETRACK 
HOVERCONTROL 
WAYPOINTCONTROL 
FOLLOWWAYPOINTMODE 
LATERALCONTROL 
ROTATECONTROL 
REPORTSTABLE 
NEWTARGETSTATION 
t  a  r ge  t_r ange_c  ommand 
targe t_bearing_command 
target_range 
targe t_bearing 
SONARSCANDIRECTION 

dsign  (normalizes  ( target_bearing  ~  normalize  (psi  +  AUV_ST1000_bearing) ) 

} 

else  if  (parameter spread  ==  6) 


{ 


if  (DISPLAYSCREEN) 

printf  ("\n[%s  %6.2f  %6.2f  %6,2f  %6.2f  %6.2f]\n",  keyword, 

parameterl ,  parameters , 
parameters,  parameter4, 
parameters)  ; 

TARGETCONTROL  =  TRUE; 

NEWTARGET  =  TRUE; 
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RECOVER YCONTROL 
SONARSCANMODE 
'  TARGETPOINTING 
TARGETEDGETRACK 
HOVERCONTROL 
WAYPOINTCONTROL 
FOLLOWWAYPO INTMODE 
LATERALCONTROL 
ROTATECONTROL 
REPORTSTABLE 
NEWTARGETSTAT ION 
targe  t_range_coinmand 
target_bearing_coinmand 
psi^coiranand 
targe t_range 
targe t_bearing 
SONARSCANDIRECTION 


=  FALSE; 

4;  /*  Locate  Target  with  STIOOO  * 

=  FALSE; 

=  FALSE; 

=  FALSE; 

=  FALSE; 

=  FALSE; 

=  FALSE; 

=  FALSE; 

=  TRUE; 

=  TRUE; 

=  parameters; 

=  normalize  {paraineter4)  ; 

=  normalize  (parameters) ; 

=  parameter!; 

=  normalize  (parameter2) ; 


dsign  (normalize2  ( targe t_bearing  -  normalize  (psi  +  AUV_ST1000_bearing) ) ) ; 


} 

else 


{ 

#ifndef  os9 


#else 


if  ((strcmp  (keyword,  " EDGE- STAT ION" )  ==  0)  || 

(strcmp  (keyword,  "EDGESTATION" )  ==  0)) 


parameters_read  =  sscanf  (command__buf fer,  "%s%lf%lf %lf %lf %lf " , 

keyword,  &  parameterl,  &  parameter2, 
.  &  parameters,  &  parameter4, 
&  parameters); 


#endif 


parameters_read  =  sscanf  (command__buf fer ,  "%s%F%F%F%F%F" , 

keyword,  &  parameterl,  &  parameter2, 
&  parameters,  &  parameter4. 
Sc  parameters)  ; 


if  {parameters_read  == 

{ 

if  (DISPLAYSCREEN) 
printf  ("\n[%s 

TARGETCONTROL 

NEWTARGET 

RECOVERYCONTROL 

TARGETPOINTING 

TARGETEDGETRACK 

HOVERCONTROL 

WAYPOINTCONTROL 

FOLLOWWAYPO INTMODE 

LATERALCONTROL 

ROTATECONTROL 

REPORTSTABLE 

NEWTARGETSTAT I ON 


3) 


%6.2f  %6.2f]\n",  keyword,  parameterl, 
parameter2 ) ; 

=  TRUE; 

=  TRUE; 

=  FALSE; 

=  TRUE; 

=  TRUE; 

=  FALSE; 

=  FALSE; 

=  FALSE; 

=  FALSE; 

=  FALSE; 

=  TRUE; 

=  .TRUE; 


targe  t__range_coinmand  =  parameterl; 

targe t_bearing_command  =  normalize  (parameter2) ; 

} 

else  if  (parameters_read  ==  4) 


{ 


if  (DISPLAYSCREEN) 
printf  ("\n[%s 


TARGETCONTROL 

NEWTARGET 

RECOVERYCONTROL 

TARGETPOINTING 

TARGETEDGETRACK 

HOVERCONTROL 

WAYPOINTCONTROL 

FOLLOWWAYPO INTMODE 


%6.2f  %6.2f  %6.2f]\n",  keyword, 

parameterl ,  parameters , 
parameters); 

=  TRUE; 

=  TRUE; 

=  FALSE; 

=  FALSE; 

=  TRUE; 

=  FALSE; 

=  FALSE; 

=  FALSE; 


/ 
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LATERALCONTROL 

ROTATECONTROL 

REPORTSTABLE 

NEWTARGETSTATION 


FALSER- 

FALSE; 

TRUE; 

TRUE; 


targe t_range_command  =  parameter!; 
target_bearing_coinmand  =  normalize  (parameter2 )  ; 
psi^command  =:  normalize  (parameters); 

} 

else  if  (parameters_read  ==  5) 

{ 


if  (DISPLAYSCREEN) 
printf  {"\n[%s 


TARGETCONTROL 
NEWTARGET 
RECOVERYCONTROL 
SONARSCANMODE 
TARGETPOINTING 
TARGETEDGETRACK 
HOVERCONTROL 
WAYPOINTCONTROL 
FOLLOWWAYPO INTMODE 
LATERALCONTROL 
ROTATECONTROL 
REPORTSTABLE 
NEWTARGETSTATION 


%6.2f  %6.2f  %6.2f  %6.2f]\n"R  keyword, 

parameter!,  parameters, 
parameters ,  parameter4 ) ; 

=  TRUE; 

=  TRUE; 

=  FALSE ; 

=  4;  /*  Locate  Target  with  ST!000  */ 

=  TRUE; 

=  TRUE; 

=  FALSE; 

=  FALSE; 

=  FALSE; 

=  FALSE; 

=  FALSE; 

=  TRUE; 

=  TRUE; 


targe  t_range_command  =  parameters; 

targe t_bearing_coinmand  =  normalize  (parameter4)  ; 

target_range  =  parameter!; 

target__bearing  =  normalize  (parameters); 

SONARSCANDIRECTION 


dsign  (normalizes  (target_bearing  -  normalize  (psi  +  AUV_ST1000_bearing) ) ) ; 

} 

else  if  (parameters_read  ==  6) 

{ 


if  (DISPLAYSCREEN) 
printf  ("\n[%s  %6.2f 


TARGETCONTROL 

NEWTARGET 

RECOVERYCONTROL 

SONARSCANMODE 

TARGETPOINTING 

TARGETEDGETRACK 

HOVERCONTROL 

WAYPOINTCONTROL 

FOLLOWWAYPOINTMODE 

LATERALCONTROL 

ROTATECONTROL 

REPORTSTABLE 

NEWTARGETSTATION 


%6.2f  %6.2f  %6.2f  %6,2f]\n",  keyword, 
parameter!,  parameters, 
parameters,  parameter4, 
parameters) ; 

=  TRUE; 

=  TRUE; 

=  FALSE; 

4;  /*  Locate  Target  with  ST!000  * 

=  FALSE; 

=  TRUE; 

=  FALSE; 

=  FALSE; 

=  FALSE; 

=  FALSE ; 

=  FALSE; 

=  TRUE; 

=  TRUE; 


target_range_command  =  parameters; 

targe t__bearing_command  =  normalize  (parameter 4)  ; 

psi_command  =  normalize  (parameters)  ; 

target_range  =  parameter!; 

target_bearing  =  normalize  (parameters) ; 

SONARSCANDIRECTION 


/ 


dsign  (normalizes  ( targe t_bearing  -  normalize  (psi  +  AUV_ST1000_bearing) ) ) ; 


} 

else  if 


{ 

#ifndef  os9 


((strcmp  (keyword,  "ENTER-TUBE")  ==  0)  M 
(strcmp  (keyword,  "ENTERTUBE")  ==  0)) 


parameters_read  =  sscanf  (command_buf fer, "%s%lf %lf " , 
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#else 


keyword,  &  parameterl,  &parameter2 ) ; 


#endif 

parameter2) ; 


/*  z. 
} 

else 

{ 

#ifndef  os9 

#else 

#endif 


} 

else 

{ 

#ifndef  os9 

#else 

#endif 

parameterl) ; 


paranieters_read 


sscanf  {coinmand_buf  fer,  "%s%F%F"  , 

keyword,  &  parameterl,  &parameter2 )  ; 


printf  {"[%s  %5.31f  %5 . 31 f] \n", keyword,  parameterl, 

SONARSCANMODE  =  5 ; 
stlOOO_command  =  -SONARHEADINGSTEP  * 

{(int)  (normalize (75.0)  /  SONARHEADINGSTEP)); 
HOVERCONTROL  =  FALSE; 

WAYPOINTCONTROL  =  FALSE; 

LATERALCONTROL  =  FALSE; 

ROTATECONTROL  =  FALSE; 

TARGETCONTROL  =  FALSE; 

RECOVER YCONTROL  =  TRUE; 

REPORTSTABLE  =  TRUE; 

NEWRECOVERYCOMMAND  =  TRUE; 

range_from_recovery_pt  =  parameterl; 
psi_command  =  normalize  (parameter2) ; 

psi_command_hover  =  psi_command; 
x_command  =  x; 

y__command  =  y; 

.command  =  z;  not  needed,  use  previously  ordered  value  */ 


if  ((strcmp  (keyword, 

(strcmp  (keyword, 


"WAIT")  ==0)  M 
"RUN")  ==  0)) 


parameters_read  =  sscanf  (command_buf fer,  "%s%lf", 

keyword,  &  parameterl); 

parameters_read  =  sscanf  (command_buf fer,  "%s%F", 

keyword,  &  parameterl); 


if  (DISPLAYSCREEN) 

printf  ("\n[%s  %6.2f;  ",  keyword,  parameterl); 

if  ( (parameters_read  ==  2)  &&  (parameterl  >=  0.0)) 

{ 

if  (TACTICALPARSE)  return  (FALSE); 

read_another_line  =  FALSE; 
time_next_command  =  t  +  parameterl; 
if  (DISPLAYSCREEN)  printf  ("time  of  next  command  %6.2f]\n", 
^  time_next_command)  ; 

else  if  (DISPLAYSCREEN) 

printf  ("  warning:  illegal  time  value,  ignoredXn") ; 

if  ((strcmp  (keyword,  "TIME")  ==  0)  il 

(strcmp  (keyword,  "WAITUNTIL")  ==0)  1  I 
(strcmp  (keyword,  "PAUSEUNTIL" )  ==  0)) 


parameters_read  =  sscanf  (command_buf fer ,  "%s%lf", 

keyword,  &  parameterl); 

parameters_read  =  sscanf  (command_buf f er,  "%s%F", 

keyword,  &  parameterl); 

if  (DISPLAYSCREEN)  printf  ("\n[%s  %6.2fJ\n",  keyword, 

if  (parameters_read  ==  2) 

{ 

if  (TACTICALPARSE)  return  (FALSE); 
read_another_line  =  FALSE; 
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#ifndef 

#else 

#endif 


tiine_next__cominand  =  parameterl ; 
if  (parameterl  <=  t) 

{ 

t  =  parameterl; 
if  (DISPLAYSCREEN) 

{ 

printf  ("  warning:  time  value  has  reset  AUV  clock/'); 
printf  ("  velocities  reset  to  zero.Xn"); 

} 


u 

= 

0.0; 

V 

= 

0.0; 

w 

= 

0.0; 

p 

= 

0.0; 

q 

= 

0.0; 

r 

= 

0.0; 

x_dot 

= 

0.0; 

y_dot 

= 

0.0; 

Z— dot 

0.0; 

phi_dot 

= 

0.0; 

theta_dot 

= 

0.0; 

psi_dot 

= 

0.0; 

read_another_line  =  TRUE;  /*  no  PDU  */ 

} 

} 

else  if  (DISPLAYSCREEN) 

printf  ("  warning:  illegal  time  value,  ignored. \n" ) ; 

} 

else  if  ( (strcmp  (keyword,  "TIMESTEP" )  ==  0)  M 

(strcmp  (keyword,  "TIME-STEP")  ==  0))  /*  different  than  STEP  */ 

{ 

os9 

if  (sscanf  (command__buf fer ,  "%s%lf",  keyword,  &parameterl)  ==  2) 
if  (sscanf  (command_buf f er,  "%s%F",  keyword,  &parameterl)  ==  2) 

{ 

if  (TACTICALPARSE)  return  (FALSE); 

if  ((parameterl  >0.0)  &&  (parameterl  <=  5.0)) 

{ 

dt  =  parameterl; 
if  (DISPLAYSCREEN) 

printf  {"\n[TIMESTEP  %6,2f]  ",  dt) ; 
if  (TACTICALPARSE  ==  FALSE) 

if  (NOSCRIPT  ==  FALSE)  fprintf  (auvordersf ile, 

"#  timestep:  %4.2f  secondsXn",  dt) ; 

} 

else  print_help  =  TRUE; 

} 

else  print_help  =  TRUE; 

} 

else  if  ((strcmp  (keyword,  "PAUSE")  ==  O)  II 
(strcmp  (keyword,  "-PAUSE")  ==  0)) 

{ 

if  (DISPLAYSCREEN) 

{ 

printf  ("\n[PAUSE]\n") ; 

strcpy  (buffer,  "  Press  any  key  to  continue"); 
send_buffer_to_virtual_world_socket  () ;  /*  buffer  msg  sent  */ 
printf  ("\n%s  ***  HERE  ***:  ",  buffer); 

answer  =  getchar  ();  /*  pause  */ 

} 

} 

else  if  ((strcmp  (keyword,  "REALTIME")  ==  0)  II 
(strcmp  (keyword,  "REAL-TIME")  ==  0)) 

{ 

if  (DISPLAYSCREEN)  printf  (" \n [REALTIME]  "); 

REALTIME  =  TRUE; 
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} 


else  if  ( (strcmp  {keyword,  "MISSION")  ==  0)  M 

(strcmp  (keyword,  "SCRIPT")  ==  0)  M 

(strcmp  (keyword,  "FILE")  ==0)  11 

(strcmp  (keyword,  "FILENAME")  ==  0)) 


parameters_read  =  sscanf  (command^buff er,  "%s%s", 

keyword,  new_f ilename)  ; 

if  (parameters_read  ==  2) 

{ 

if  (DISPLAYSCREEN) 

printf  ("\n[%s  %s]\n",  keyword,  new_filename) ; 

#ifndef  os9 


#else 


sprintf  (backupcommand,  "cp  %s  %s",  new_f ilename, 
AUVSCRIPTFILENAME)  ; 


#endif 


sprintf  (backupcommand,  "copy  %s  %s",  new_f ilename, 
AUVSCRIPTFILENAME)  ; 


if  (DISPLAYSCREEN) 

printf  {"%s\n",  backupcommand); 
system  (  backupcommand); 

auvscriptfile  ==  NULL;  /*  force  re-read  */ 

} 

else 


{ 

if  (DISPLAYSCREEN) 

printf  ("\n[%s]  warning:  no  filename  present,  ignoredXn",  keyword); 
} 

} 

else  if  ((strcmp  (keyword,  "KEYBOARD")  ==0)  II 

(strcmp  (keyword,  " KEYBOARD- ON" )  ==  0)  M 

(strcmp  (keyword,  "KEYBOARD -INPUT")  ==  0)  I  I 

(strcmp  (keyword,  " KEYBOARD INPUT " )  ==  0)) 

{ 

if  (DISPLAYSCREEN)  printf  ("\n[%s]\n",  keyword); 

KEYBOARD INPUT  =  TRUE; 

} 

else  if  ((strcmp  (keyword,  " KEYBOARD- OFF " )  ==  0)  I  I 

(strcmp  (keyword,  "NO-KEYBOARD")  ==  0)) 

{ 

if  (DISPLAYSCREEN)  printf  ("\n[%s]\n",  keyword); 


KEYBOARDINPUT  =  FALSE; 

) 

else  if  ((strcmp  (keyword,  "NOWAIT")'  ==  0) 

(strcmp  (keyword,  "NO-WAIT")  ==  0) 

(strcmp  (keyword,  "NOREALTIME")  ==  0) 

(strcmp  (keyword,  "NO-REALTIME" )  ==  0) 

(strcmp  (keyword,  "NONREALTIME")  ==  0) 

(strcmp  (keyword,  "NO-PAUSE")  ==  0) 

(strcmp  (keyword,  "NOPAUSE")  ==  0)) 

{ 


if  (DISPLAYSCREEN)  printf  ("\n[%s]\n",  keyword); 

REALTIME  =  FALSE; 

} 

else  if  ((strcmp  (keyword,  "ABORT")  ==  0)) 

{ 

HALTSCRIPT  =  TRUE; 

if  (DISPLAYSCREEN)  printf  ("\n[%s]\n",  keyword); 

} 

else  if  ((strcmp  (keyword,  "QUIT")  ==  0)  !l 

(strcmp  (keyword,  "STOP")  ==  0)  I ! 

(strcmp  (keyword,  "DONE")  ==  0)  M 

(strcmp  (keyword,  "EXIT")  ==  0)  II 

(strcmp  (keyword,  "REPEAT")  ==  0)  11 

(strcmp  (keyword,  "RESTART")  ==  0)  I i 

(strcmp  (keyword,  "COMPLETE")  ==  0)  11 
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(strcmp  {keyword,  "KILL'')  ==  0)  M 

(strcmp  (keyword,  "SHUTDOWN")  ==  0)) 

/*  note  most  of  these  commands  don't  reset  LOOPFOREVER,  except  */ 
/*  KILL/SHUTDOWN  which  terminate  the  dynamics  model  connection  */ 

if  ((strcmp  (keyword,  "KILL")  ==  0)  II 

(strcmp  (keyword,  "SHUTDOWN")  ==  0)) 

{ 

LOOPFOREVER  =  FALSE; 
strcpy  (buffer,  "KILL"); 

send_buf fer_to_virtual_world_socket  ();  /*  buffer  msg  sent  */ 

} 

else 

{ 

strcpy  (buffer,  "QUIT"); 

send_buffer_to_virtual_world_socket  {);  /*  buffer  msg  sent  */ 

} 


printf {"\n[QUIT  condition: 
closed] \n" , 


if  (DISPLAYSCREEN)  printf  ("\n[%s]\n",  keyword); 

if  (TRACE  &&  DISPLAYSCREEN)  printf  ( "\n[end_test  set  TRUE]\n"); 

end_test  =  TRUE; 

read_another_line  =  FALSE; 

if  (TACTICALPARSE)  return  (FALSE); 

fclose  (auvscriptf ile) ; 
auvscriptf ilequit  =  TRUE; 
if  (DISPLAYSCREEN) 


} 

else 


{ 


#ifndef  os9 


#else 


#endif 


(%s  backup  file)  mission. script .backup,  file 
AUVSCRI PTFILENAME ) 


x_dot 

0.0; 

y_dot 

= 

0.0; 

z_dot 

= 

0.0; 

phi__dot 

0.0; 

/* 

degrees/sec 

*/ 

theta_dot 

0.0; 

/* 

degrees/sec 

*/ 

psi_dot 

= 

0.0; 

/* 

degrees/sec 

*/ 

speed 

= 

0.0; 

u 

= 

0.0; 

V 

= 

0.0; 

w 

0.0; 

p 

= 

0.0; 

/* 

degrees/sec 

*/ 

q 

= 

0.0; 

/* 

degrees /sec 

*/ 

r 

= 

0.0; 

/* 

degrees /sec 

*/ 

delta_p lanes 

=: 

0.0; 

/* 

degrees 

*/ 

delta_rudder 

0.0; 

/* 

degrees 

V 

port_rpm 

0; 

stbd_rpm 

= 

0; 

vGrtical_thruster_volts 

0.0; 

lateral_thruster_volts 

= 

0.0; 

return  (FALSE) ; 

if  (  (strcmp  (keyword. 

"RPM" ) 

==  0)  1  i 

(strcmp  (keyword. 

"SPEED") 

==  0)  !  1 

(strcmp  (keyword, 

"PROPS") 

==  0)  11 

( strcmp  ( key wo rd , 

"PROPELLORS")  ==  0)) 

pa  rame  ters_read 


sscanf  (command_buf fer,  "%s%lf%lf", 

keyword,  &  parameterl, 
&  parameter2); 


parameters_read  =  sscanf  (command^buf f er,  "%s%F%F", 

keyword,  &  parameterl, 
&  parameter2); 

if  (parameters_read  ==  3) 
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paraineter2 )  ; 

#ifndef  os9 

#else 

#endif 


} 

else 

{ 

#ifndef  os9 

#else 

■#endi f 
parameterl ) ; 


} 

else 


WAYPOINTCONTROL  =  FALSE; 

ROTATECONTROL  =  FALSE; 

HOVERCONTROL  =  FALSE; 

TARGETCONTROL  =  FALSE; 

RECOVER YCONTROL  =  FALSE; 

SONARSCANMODE  =1;  /*  Forward  Scan  */ 

if  (DISPLAYSCREEN) 

printf  ("\n[%s  %6.2f  %6 . 2f  ]  \n" ,  keyword,paraineterl , 

port_rpin_coiranand  =  parameter!; 
stbd_rpm__command  =  parameter2; 

} 

else 

{ 

parameters_read  =  sscanf  (command_buf f er,  "%s%lf'\ 

keyword, parameter!}; 

parameters_read  =  sscanf  (coinraand_buf fer,  "%s%F", 

keyword,  &  parameter!); 

if  (DISPLAYSCREEN) 

printf  ("\n[%s  %6.2f]\n",  keyword,  parameter!); 

if  (parameters_read  ==  2) 

{ 

WAYPOINTCONTROL  =  FALSE; 

ROTATECONTROL  =  FALSE; 

HOVERCONTROL  =  FALSE; 

TARGETCONTROL  =  FALSE; 

RECOVERYCONTROL  =  FALSE; 

SONARSCANMODE  =  !;  /*  Forward  Scan  */ 

port_rpm_command  =  parameter!; 
s  t  bd_rpm_c  ommand  =  par  ame  t  e  r !  ; 

} 

else  if  (DISPLAYSCREEN)  printf  ("  warning;  no  value,  ignoredXn") ; 

if  ( (strcmp  (keyword,  "COURSE")  ==  0)  M 

(strcmp  (keyword,  "HEADING")  ==0)  II 

(strcmp  (keyword,  "YAW")  ==0)) 


parameters_read  =  sscanf  (command_buf fer,  "%s%lf", 

keyword,  &  parameter!) 

parameters_read  =  sscanf  (command_buf fer,  "%s%F", 

keyword,  &  parameter!); 

if  (DISPLAYSCREEN)  printf  ("\n[%s  %6.2fj\n",  keyword, 

if  (parameters_read  ==  2) 

{ 

DEADSTICKRUDDER  =  FALSE; 

WAYPOINTCONTROL  =  FALSE; 

TARGETPOINTING  =  FALSE; 

psi^command  =  normalize  (parameter!); 

psi_command_hover  =  psi_command; 

rot a te_c ommand  =  0.0; 

la ter a l_c ommand  =  0.0; 

ROTATECONTROL  =  FALSE; 

LATERALCONTROL  =  FALSE; 

REPORTSTABLE  =  TRUE; 

.  } 

else  if  (DISPLAYSCREEN)  printf  ("  warning:  no  value,  ignored\n"); 

if  ((strcmp  (keyword,  "PITCH")  ==0)  )| 

(strcmp  (keyword,  "THETA")  ==  0)) 
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{ 

#ifndef  os9 

#else 

#endif 

parameterl) ; 


} 

else 

{ 

#ifndef  os9 

#else 

#endif 

parameterl )  ; 

} 

else 

{ 

#ifndef  os9 

#else 

#endif 

parameterl ) ; 


parameters_read  =  sscanf  (command_buf fer,  "%s%lf", 

keyword,  &  parameterl) 

parameters_read  =  sscanf  (command^buf f er,  "%s%F", 

keyword,  &  parameterl) 

if  (DISPLAYSCREEN)  printf  {"\n[%s  %6.2f]\n",  keyword, 

if  (parameters_read  ==  2) 

{ 

DEADSTI CKRUDDER  =  FALSE ; 

WAYPOINTCONTROL  =  FALSE; 

TARGETPOINTING  =  FALSE; 

theta__coiranand  =  normalize  (parameterl); 
rotate_coinmand  =  0.0; 

lateral_command  =  0.0; 

ROTATECONTROL  =  FALSE; 

LATERALCONTROL  =  FALSE; 

REPORTSTABLE  =  TRUE; 

} 

else  if  (DISPLAYSCREEN)  printf  ("  warning:  no  value,  ignored\n") 

if  ( {strcmp  (keyword,  "TURN")  ==  0)  I! 

(strcmp  (keyword,  " CHANGE -COURSE" )  ==  0)) 

parameters_read  =  sscanf  (command_buf fer,  "%s%lf", 

keyword,  &  parameterl) 

parameters_read  =  sscanf  (command_buf f er,  "%s%F", 

keyword,  &  parameterl) 

if  (DISPLAYSCREEN)  printf  ("\n[%s  %6.2f]\n",  keyword, 

if  (parameters_read  ==  2) 

{ 

DEADSTI CKRUDDER  =  FALSE; 

WAYPOINTCONTROL  =  FALSE; 

ROTATECONTROL  =  FALSE; 

TARGETPOINTING  =  FALSE; 

psi__command  =  normalize  (psi_command  +  parameterl); 

} 

else  if  (DISPLAYSCREEN)  printf  ("  warning:  no  value,  ignoredXn") 
if  (strcmp  (keyword,  "RUDDER")  ==  0) 


parameters_read  =  sscanf  (command_buff er,  "%s%lf", 

keyword,  &  parameterl) 

parameters_read  =  sscanf  (command_buff er,  "%s%F", 

keyword,  &  parameterl) 

if  (DISPLAYSCREEN) 

printf  ("\n[%s  %6.2f,  THRUSTERCONTROL  ==  FALSE] \n",  keyword, 

if  (parameters__read  ==  2) 

{ 

DEADSTICKRUDDER  =  TRUE; 

WAYPOINTCONTROL  =  FALSE; 

ROTATECONTROL  =  FALSE; 

HOVERCONTROL  =  FALSE; 

TARGETCONTROL  =  FALSE; 

RECOVER YCONTROL  =  FALSE; 

THRUSTERCONTROL  =  FALSE; 

SONARSCANMODE  =1;  /*  Forward  Scan  */ 
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/*  note  rudder_command  is  for  forward  rudder,  negative  com¬ 
mand  turns  left  */ 

rudder_command  =  normalize2  {-  parameter!); 

} 

else 

{ 

if  (DISPLAYSCREEN) 

printf  ("  warning:  improper  value,  rudder  order  ignored\n" ) ; 

} 

} 

else  if  (strcmp  (keyword,  "DEADSTICKRUDDER" )  ==  0) 

{ 

#ifndef  os9 


#else 


parameters_read  =  sscanf  (command__buf  f  er ,  "%s%lf", 

keyword,  &  parameter!); 


#endif 


parameters_read  =  sscanf  (command_buf fer,  "%s%F", 

keyword,  &  parameter!); 


if  (parameters_read  ==  2) 

{ 

if  (DISPLAYSCREEN) 

printf  ("\n[%s  %6.2f,  THRUSTERCONTROL  ==  FALSE] \n", 

keyword,  parameterl ) ; 

DEADSTICKRUDDER  =  TRUE; 

WAYPOINTCONTROL  =  FALSE; 

ROTATECONTROL  =  FALSE; 

HOVERCONTROL  =  FALSE; 

TARGETCONTROL  =  FALSE; 

RECOVER YCONTROL  =  FALSE; 

THRUSTERCONTROL  =  FALSE; 

SONARSCANMODE  =1;  /*  Forward  Scan  */ 

rudder_command  =  normali2e2  (parameterl)  ; 

} 

else 

{ 

if  (DISPLAYSCREEN)  printf  ("\n[%s,  ]  ",  keyword); 

DEADSTICKRUDDER  =  TRUE; 

WAYPOINTCONTROL  =  FALSE; 

ROTATECONTROL  =  FALSE; 
rudder_command  =  0,0; 
if  (DISPLAYSCREEN) 

printf {"  warning:  improper/missing  value,  rudder  set  to  0\n"); 

} 

} 

else  if  (strcmp  (keyword,  "DEPTH")  ==  0) 

{ 


#ifndef  os9 


#else 


#endif 

parameterl) ; 


parameters_read  =  sscanf  (command_buf f er,  "%s%lf", 

keyword.  Sc  parameterl); 


parameters_read  =  sscanf  (coramand_buf fer,  "%s%F", 

keyword,  &  parameterl); 

if  (DISPLAYSCREEN)  printf  ("\n[%s  %6.2f]\n",  keyword, 

if  (parameters_read  ==  2) 

{ 

DEADSTICKPLANES  =  FALSE; 

INTEGRALDEPTHCONTROL  =  FALSE; 

TARGETCONTROL  =  FALSE; 

time_int__control_on  =  t  +  10.0;  /*  give  PD  10  seconds  */ 
z_command  =  parameterl; 


} 


if  (HOVERCONTROL)  /*  report  when  stable  again 
REPORTSTABLE  =  TRUE; 


else  if  (DISPLAYSCREEN)  printf  ("  warning:  no  value. 


*/ 

ignoredXn") ; 
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} 

else 


if  (strcmp  (keyword, 


PLANES") 


0) 


#ifndef  os9 


#else 


#endif 


parameters_read  =  sscanf  (command^buf f er ,  "%s%lf", 

keyword,  &  parameter!); 

parameter spread  =  sscanf  (command_buf fer ,  "%s%F", 

keyword,  &  parameterl); 


if  (DISPLAYSCREEN) 

printf  ("\n[%s  %6.2f,  THRUSTERCONTROL  ==  FALSE] \n", 
keyword,  parameterl); 
if  (parameters_read  ==  2) 

{ 

DEADSTICKPLANES  =  TRUE; 

THRUSTERCONTROL  =  FALSE; 

/*  note  planes_command  is  for  forward  planes,  negative  command  points  down  */ 
planes_command  =  -  parameterl; 

} 

else  if  (DISPLAYSCREEN) 

^  printf  ("  warning:  improper  value,  planes  order  ignoredXn"); 

else  if  (strcmp  (keyword,  "DEADSTICKPLANES")  ==  0) 


#ifndef  os9 


#endif 


parameters_read  =  sscanf  (command_buf f er ,  "%s%lf", 

keyword,  &  parameter!); 

parameters_read  =  sscanf  (command_buf f er ,  "%s%F", 

keyword,  &  parameterl); 

if  {parameters_read  ==  2) 

{ 

if  (DISPLAYSCREEN) 

printf  ("\n[%s  %6.2f,  THRUSTERCONTROL  ==  FALSE] \n", 
keyword,  parameterl) ; 

DEADSTICKPLANES  =  TRUE; 

THRUSTERCONTROL  =  FALSE; 
planes_coinmand  =  parameterl; 

} 

else 

{ 

if  (DISPLAYSCREEN)  printf  ("\n[%s]  ",  keyword); 

DEADSTICKPLANES  =  TRUE; 
planes_command  =  0.0; 
if  (DISPLAYSCREEN) 

^  printf  ("  warning:  improper  value,  planes  set  to  0\n"); 


else  if  ((strcmp  (keyword,  "THRUSTERS -ON")  ==  0)  M 

(strcmp  (keyword,  "THRUSTERS")  ==  0)  II 

(strcmp  (keyword,  "THRUSTERON" )  ==  0)  i 1 

(strcmp  (keyword,  "THRUSTERSON" )  ==  0)) 

if  (DISPLAYSCREEN)  printf  {"\n[%s]\n",  keyword); 

THRUSTERCONTROL  =  TRUE; 

} 

else  if  ((strcmp  (keyword,  "NOTHRUSTER")  ==  0)  M 

(strcmp  (keyword,  "NOTHRUSTERS")  ==  O)  II 

(strcmp  (keyword,  "THRUSTERS-OFF" )  ==  0)  II 

(strcmp  (keyword,  "THRUSTERSOFF" )  ==  0)) 

if  (DISPLAYSCREEN)  printf  {"\n[%s]\n",  keyword); 

THRUSTERCONTROL  =  FALSE; 

ROTATECONTROL  =  FALSE; 

LATERALCONTROL  =  FALSE; 

HOVERCONTROL  =  FALSE; 
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} 

else 

{ 

#ifndef  os9 


#else 


#endif 

parameterl) ; 


} 

else 


{ 


} 

else 

{ 

#ifndef  os9 


#elsG 


#endif 


} 

else 


TARGETCONTROL  =  FALSE; 

RECOVERYCONTROL  =  FALSE; 

SONARSCANMODE  =1;  /*  Forward  Scan  */ 

if  (strcmp  (keyword,  "ROTATE")  ==  0) 


parameters_read  =  sscanf  {command_buf fer,  "%s%lf", 

keyword,  &  parameterl); 

parameters_read  =  sscanf  (command^buf fer,  "%s%F", 

keyword,  &  parameterl); 

if  (DISPLAYSCREEN)  printf  {"\n[%s  %6.2f]\n",  keyword, 

if  (parameters_read  ==  2) 

{ 

THRUSTERCONTROL  =  TRUE; 

WAYPOINTCONTROL  =  FALSE; 

HOVERCONTROL  =  FALSE; 

TARGETCONTROL  =  FALSE; 

RECOVERYCONTROL  =  FALSE; 

SONARSCANMODE  =1;  /*  Forward  Scan  */ 

rotate_command  =  parameterl; 
clamp  {&rotatG_command,  -12.0,  12.0,  "rotate_command" ) ; 
lateral^command  =  0.0; 

ROTATECONTROL  =  TRUE; 

LATERALCONTROL  =  FALSE; 

} 

else  if  (DISPLAYSCREEN)  printf  ("  warning:  no  value,  ignoredXn")  ; 

if  ({strcmp  (keyword,  "NOROTATE")  ==  0)  11 

(strcmp  (keyword,  "ROTATEOFF")  ==  0)  M 

(strcmp  (keyword,.  "  ROTATE  -  0  FF " )  ==  0)) 

if  (DISPLAYSCREEN)  printf  ("\n[%s]\n",  keyword); 
rotate^cdmmand  =  0.0; 

ROTATECONTROL  =  FALSE; 

if  (strcmp  (keyword,  "LATERAL")  ==  0) 


parameters_read  =  sscanf  (command_buf f er,  "%s%lf"., 

keyword,  &  parameterl); 

parameters_read  =  sscanf  (command_buf f er ,  "%s%F", 

keyword,  &  parameterl ) ; 

if  (DISPLAYSCREEN)  printf  ("\n[%s  %6.2f]\n", 

keyword,  parameterl); 

if  (parameters_read  ==  2) 

{ 

THRUSTERCONTROL  =  TRUE; 

WAYPOINTCONTROL  =  FALSE; 

HOVERCONTROL  =  FALSE ; 

TARGETCONTROL  =  FALSE; 

RECOVERYCONTROL  =  FALSE; 

SONARSCANMODE  =1;  /*  Forward  Scan  */ 

rotate_command  =  0.0; 
lateral_command  =  parameterl; 

ROTATECONTROL  =  FALSE; 

LATERALCONTROL  =  TRUE; 

} 

else  if  (DISPLAYSCREEN)  printf  ("  warning:  no  value,  ignoredXn"); 

•if  ((strcmp  (keyword,  "NOLATERAL")  ==  0)  II 

(strcmp  (keyword,  "LATERALOFF" )  ==  0)  II 

(strcmp  (keyword,  " LATERAL- OFF " )  ==  0)) 
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} 

else 


if  (DISPLAYSCREEN)  printf  ("\n[%s]\n",  keyword); 
lateral_coitiinand  =  0.0; 

LATERALCONTROL  =  FALS  E ; 


if  ((strcmp  (keyword, 
(strcmp  (keyword, 
(strcmp  (keyword, 
(strcmp  (keyword, 
(strcmp  (keyword. 


"DIVETRACKERl")  ==0)  1| 

"DIVE-TRACKERl")  ==  0)  11 

"DIVE-TRACKER- 1'')  ==0)  M 

"DIVE^TRACKERl")  ==  0)  li 

'DIVE__TRACKER_1")  ==  0)  ) 


/*  note  this  command  must  be  sent  to  virtual  world  (AUVsocket.C  tests)  */ 
{ 


#ifndef  os9 


#else 


parameters_read  =  sscanf  (command_buffer,  "%s%lf%lf%lf ", 

keyword,  &  parameterl, 

&  parameter2,  &  parameters); 


#endif 


parameters_read  =  sscanf  (command_buffer,  "%s%F%F%F", 

keyword,  &  parameterl. 

Sc  parameter2,  &  parameters); 


if  (DISPLAYSCREEN) 

printf  ("\n[%s  %6.2f  %6.2f  %6.2f]\n", 

key wo  rd ,  pa  rame  t  e  r 1 , 

parameter2 ,  parameters ) ; 

if  (parameters_read  ==  4) 

{ 

DiveTrackerl_x  =  parameterl; 

DiveTrackerl^  =  parameters,- 
DiveTrackerl„z  =  parameters,- 


if  (TRACE  &&  DISPLAYSCREEN) 

printf  ("\nsending  divetracker  data  to  virtual  world:  [%s]\n",  buffer); 

strcpy  (buffer,  command_buffer) ;  /*  copy  command  to  buffer*/ 
send__buf fer_to_yirtual_world_socket  ();  /*  send  to  vw  */ 

} 

else  if  (DISPLAYSCREEN) 

^  printf  ("  warning:  invalid  DiveTrackerl  position,  ignoredXn") ; 

else  if  ((strcmp  (keyword,  "DIVETRACKER2 " )  ==  0)  II 

(strcmp  (keyword,  "DIVE-TRACKER2" )  ==  0)  II 

(strcmp  (keyword,  "DIVE-TRACKER-2" )  ==0)  M 

(strcmp  (keyword,  "DIVE_TRACKER2" )  ==  0)  1| 

(strcmp  (keyword,  "DIVE_TRACKER__2")  ==  0)  ) 

/*  note  this  command  must  be  sent  to  virtual  world  (AUVsocket.C  tests)  */ 

#ifndef  os9 


#else 


parameters_read  =  sscanf  (command_buff er,  "%s%lf %lf%lf ", 

keyword,  &  parameterl, 

&  parameters,  &  parameters); 


#endif 


parameters_read  =  sscanf  (command_buf f er ,  "%s%F%F%F", 

keyword,  &  parameterl, 

Sc  parameters,  &  parameters); 

if  (DISPLAYSCREEN) 

printf  ("\n[%s^  %6.2f  %6.2f  %6.2f]\n", 

keyword,  parameterl , 

parameters,  parameters) ; 

if  (parameters_read  ==  4) 

{ 

DiveTracker2_x  =  parameterl; 

DiveTracker2_y  =  parameters; 

DiveTracker2_z  =  parameters; 


if  (TRACE  ScSc  DISPLAYSCREEN) 

printf  {"\nsending  divetracker  data  to  virtual  world:  [%s]\n",  buffer); 

strcpy  (buffer,  command_buf fer)  ;  /*  copy  command  to  buffer*/ 
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} 

else 

{ 

#ifndef  os9 

#else 

#endif 

parameterl) ; 

} 

else 


{ 

#ifndef  os9 

#else 

#endif 

parameterl) ; 

} 

else 

{ 

#ifndef  os9 

#else 


#endif 


parameterl. 


send_buf fer_to__virtual_world_socket  ();  /*  send  to  vw  */ 

} 

else  if  (DISPLAYSCREEN) 

printf  ("  warning:  invalid  DiveTrack€r2  position,  ignored\n"); 

if  ((strcmp  (keyword,  "GYROERROR")  ==  0)  M 
(strcmp  (keyword,  "GYRO-ERROR")  ==  0)  M 
(strcmp  (keyword,  "GYRO_ERROR" )  ==  0)) 


parameters_read  =  sscanf  (command_buf f er ,  "%s%lf", 

keyword,  &  parameterl); 

parameters_read  =  sscanf  (command_buffer,  "%s%F", 

keyword,  &  parameterl); 

if  (DISPLAYSCREEN)  printf  ("\n[%s  %6.2f]\n",  keyword, 

if  (parameters_read  ==  2) 

{ 

gyro_error  =  parameterl; 

} 

else  if  (DISPLAYSCREEN) 

printf  ("warning:  invalid  GYRO-ERROR  command,  ignored\n"); 


if  ((strcmp 

(keyword. 

"DEPTH-CELL-BIAS" ) 

0) 

(strcmp 

(keyword. 

"DEPTHCELLBIAS") 

=  = 

0) 

(strcmp 

(keyword. 

"DEPTH-CELL- ERROR" ) 

=  = 

0) 

(strcmp 

(keyword. 

"DEPTHCELLERROR" ) 

=  = 

0) 

(strcmp 

(keyword. 

"DEPTH-BIAS") 

=  = 

0) 

(strcmp 

(keyword. 

"DEPTHBIAS") 

=  = 

0) 

(strcmp 

(keyword. 

"DEPTH-ERROR") 

0) 

(strcmp 

(keyword. 

"DEPTHERROR") 

=  = 

0)) 

parameters_read  =  sscanf  (command_buf fer,  "%s%lf", 

keyword,  Sc  parameterl); 

parameters_read  =  sscanf  (command_buf fer,  "%s%F", 

keyword,  &  parameterl); 

if  (DISPLAYSCREEN)  printf  ("\n[%s  %6.2f]\n",  keyword, 

if  (parameters_read  ==  2) 

{ 

depth_cell_bias  =  parameterl; 

} 

else  if  (DISPLAYSCREEN) 

printf  ("  warning:  invalid  DEPTH-CELL-BIAS  command,  ignoredXn"); 

if  ({strcmp  (keyword,  "ORIENTATION")  ==  0)  II 
(strcmp  (keyword,  "ROTATION")  ==  0)  ) 


parameters_read  =  sscanf  (command_buf fer,  "%s%lf %lf %lf " , 

keyword,  &  parameterl. 

Sc  parameter2,  &  parameters); 

parameters_read  =  sscanf  (command_buf fer,  "%s%F%F%F", 

keyword,  &  parameterl, 

&  parameter2,  &  parameters); 


if  (DISPLAYSCREEN) 

printf  ("\n[%s  %6.2f  %6.2f  %6.2f]\n",  keyword, 

parameter2 ,  parameters ) ; 

if  (parameters_read  ==  4) 

{ 
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phi  =  parameter!; 
theta  =:  parameter2  ; 
psi  =  parameters; 

} 

else 

if  (DISPLAYSCREEN) 

printf  {"  warning:  invalid  phi/theta/psi  orientation,  ignoredXn") ; 

} 

else  if  (strcmp  (keyword,  "POSTURE")  ==  0) 

{ 

#ifndef  os9 


#else 


#endif 


parameters_read  =  sscanf  (command^buf f er,  "%s%lf%lf %lf%lf %lf%lf " , 

keyword,  &  parameter!, 
Sc  parameter2,  &  parameters. 
Sc  parameter4,  &  parameters. 
Sc  parameter 6 )  ; 

parameters_read  =  sscanf  (command^buf fer,  "%s%F%F%F%F%F%F" , 

keyword,  &  parameter!. 
Sc  parameters,  &  parameters. 
Sc  parameter4,  Sc  parameters. 
Sc  parameter6); 

if  (DISPLAYSCREEN) 

printf  ("\n[%s  %6,2f  %6.2f  %6-2f  %6.2f  %6.2f  %6.2fl\n", 

keyword,  parameter!, 

parameter2,  parameters, 
parameter4,  parameters, 
parameters); 

if  (parameters_read  ==  7) 

{ 

X  =  parameter!; 

y  =  parameters; 

z  =  parameters,* 

phi  =  parameter4; 

theta  =  parameters,* 

psi  =  parameters; 

start  jpsi  =  parameters,* 
kal_init_z  =  TRUE; 

INTEGRALDEPTHCONTROL  =  FALSE; 

time_int_control_on  =  t  +  !0.0;  /*  give  PD  !0  seconds  */ 


/*  skip  line  in  telemetry  file  to  break  point-to-point  lines  */ 
if  ((TACTICAL  ==  FALSE)  I  I  (TACTICALPARSE) ) 
fprintf  (auvdatafile,  "\n"); 

} 

else  if  (DISPLAYSCREEN) 

printf ("  warning:  invalid  posture  values  (6  required),  ignored\n"); 

else  if  ((strcmp  (keyword,  "OCEANCURRENT" )  ==  0)  M 

(strcmp  (keyword,  " OCEAN- CURRENT" )  ==  0)) 

{ 

#ifndef  os9 

parameters_read  =  sscanf  (command_buf f er ,  "%s%lf %1 f%lf " , 

keyword,  &  parameter!, 

Sc  parameters,  &  parameters); 

#else 

parameters_read  =  sscanf  (command_buf fer ,  "%s%F%F%F", 

keyword,  &  parameterl. 

Sc  parameters,  &  parameters); 

#endif 

if  (parameters_read  ==  4) 

{ 

if  (DISPLAYSCREEN) 

printf  ("\n[%s  %6.2f  %6.2f  %6.2f]\n", 

keyword,  parameter!, 
parameters ,  parameters ) ; 

AUV_oceancurrent_x  =  parameter!,* 
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AUV_oceancurrent_y  =  parameter2; 

AUV_oceancurrGnt_z  =  parameters; 

} 

else  if  (parameters_read  ==  3) 

{ 

if  (DISPLAYSCREEN) 

printf  ("\n[%s  %6.2f  %6.2f]\n",  keyword,  parameterl, 

parameter2) ; 

AUV_oceancurrent_x  =  parameter!; 

AUV_oceancurrent_y  =  parameter2; 

} 

else 

{ 

if  (DISPLAYSCREEN) 

{ 

printf  ("\n  warning:  improper  number  of  OCEAN -CUR RENT  ") 
printf  ("values,  ignoredXn"); 

} 

} 

} 

else  if  ({strcmp  (keyword,  "CONTINUE")  ==  0)  II 
(strcmp  (keyword,  "GO")  ==  0)) 

{ 

if  (DISPLAYSCREEN)  printf  ("\n[%s]\n",  keyword); 
return  (FALSE);  /*  no  action  required  */ 

} 

else  if  ((strcmp  (keyword,  "STEP")  ==  0)  It 

(strcmp  (keyword,  "SINGLE-STEP" )  ==  0)) 

{ 

if  (DISPLAYSCREEN)  printf  ("\n[%s]\n",  keyword); 
time_next_command  =  t  +  dt; 
read_another_line  =  FALSE; 

} 

else  if  ((strcmp  (keyword,  "TRACE")  ==0)  II 
(strcmp  (keyword,  "TRACE-ON")  ==  0)) 

{ 

if  (DISPLAYSCREEN)  printf  ("\n[TRACE  =  TRUE]  "); 

TRACE  =  TRUE; 

} 


else  if  ((strcmp 

(keyword. 

"TRACEOFF") 

==  0) 

(strcmp 

(keyword. 

"TRACE-OFF") 

==  0) 

(strcmp 

(keyword. 

"NOTRACE") 

==  0) 

(strcmp 

(keyword. 

"NO-TRACE" ) 

==  0) 

{ 


if  (DISPLAYSCREEN)  printf  ("\n[TRACE  =  FALSE]  "); 
TRACE  =  FALSE; 

} 

#ifndef  OS9 

/*  save  space  due'  to  OS9  size  problems  */ 

else  if  ((strcmp  (keyword,  " LOOP FOREVER " )  ==  0)  11 

(strcmp  (keyword,  "LOOP -FOREVER")  ==  0)) 

{ 

if  (DISPLAYSCREEN)  printf  ("\n[LOOPFOREVER]  "); 
LOOPFOREVER  =  TRUE; 

} 

else  if  ((strcmp  (keyword,  "LOOPONCE")  ==  0)  M 
(strcmp  (keyword,  "LOOP-ONCE")  ==  0)) 

{ 

if  (DISPLAYSCREEN)  printf  (" \n [LOOPONCE]  ") ; 
LOOPFOREVER  =  FALSE; 

} 

else  if  ((strcmp  (keyword,  "LOOPFILEBACKUP" )  ==  0)  11 

(strcmp  (keyword,  "LOOP-FILE-BACKUP")  ==  0)) 

{ 

if  (DISPLAYSCREEN)  printf  (" \n [LOOPFILEBACKUP]  "); 
LOOPFILEBACKUP  =  TRUE; 

} 

else  if  ((strcmp  (keyword,  "ENTERCONTROLCONSTANTS" )  ==  0)  i 


#Gndif 


(strcmp  (keyword,  " ENTER -CONTROL- CONSTANTS " )  ==0))  • 

{ 

if  (DISPLAYSCREEN)  printf  ( "\n [ENTERCONTROLCONSTANTS]  "); 
ENTERCONTROLCONSTANTS  :r  TRUE; 
get_control_constants  (); 
fflush  (stdin); 

} 


else  if  ({strcmp  (keyword,  ''SHOWCONTROLCONSTANTS" )  ==  0)  I  I 

(strcmp  (keyword,  " SHOW- CONTROL- CONS TANTS " )  ==  0)) 

{ 

if  (DISPLAYSCREEN)  printf  ("\n  [SHOWCONTROLCONSTANTS] 
SHOWCONTROLCONSTANTS  =  TRUE; 
get_control_constants  (); 
fflush  (stdout); 


} 

else  if  ((strcmp  (keyword,  "CONTROLCONSTANTSINPUTFILE" } 
(strcmp  (keyword,  " CONTROL -CONSTANTS -IN PUT- F I LE " ) 
(strcmp  (keyword,  "CONTROL -CONSTANTS -FILE") 
(strcmp  (keyword,  "CONTROLCONSTANTSFILE" ) 

{ 


} 

else 

{ 


LOADCONTROLCONSTANTS  =  TRUE; 
get_control_constants  (); 
if  (DISPLAYSCREEN) 

printf  {"\n[CONTROLCONSTANTSINPUTFILE  %sl", 
CONTROLCONSTANTSINPUTNAME) ; 

if  ((Strcmp  (keyword,  "SLIDINGMODECOURSE" )  ==  0) 

(strcmp  (keyword,  "SLIDING-MODE-COURSE")  ==  0) 


==  0)  i I 
==  0)  M 
==  0)  1 i 

=  =  0)) 


I  i 


if  (DISPLAYSCREEN)  printf  ("\n[%s  =  TRUE]\n",  keyword); 
SLIDINGMODECOURSE  =  TRUE; 

WAYPOINTCONTROL  =  FALSE; 

ROTATECONTROL  =  FALSE; 

HOVERCONTROL  =  FALSE; 

TARGETCONTROL  =  FALSE; 

RECOVERYCONTROL  =  FALSE; 

SONARSCANMODE  =1;  /*  Forward  Scan  */ 


} 

else  if  ((strcmp  (keyword,  "SLIDINGMODEOFF" )  ==  0)  11 

(strcmp  (keyword,  "SLIDING-MODE-OFF")  ==  0)) 

{ 

if  (DISPLAYSCREEN) 

printf  ("\n[%s:  SLIDINGMODECOURSE  =  FALSE] \n",  keyword); 
SLIDINGMODECOURSE  =  FALSE; 

} 

else  if  {(strcmp  (keyword,  "SONARTRACE" )  z==  0)  II 

(strcmp  (keyword,  " SONAR- TRACE " )  ==  0)  M 

(strcmp  (keyword,  " SONAR -TRACE- ON " )  ==  0)) 

{ 

if  (DISPLAYSCREEN)  printf  (" \n (SONARTRACE]  "); 

SONARTRACE  =  TRUE; 


else  if  ((strcmp  (keyword,  "SONARTRACEOFF" )  ==  0)  11 

(strcmp  (keyword,  "SONAR-TRACE-OFF")  ==  0)) 

{ 

if  (DISPLAYSCREEN)  printf  ("\n [SONARTRACEOFF]  ") ; 
SONARTRACE  =  FALSE; 

} 

else  if  (strcmp  (keyword,  " PARALLEL PORTTRACE " )  ==  0) 

{ 

if  (DISPLAYSCREEN)  printf  ( " \n [ PARALLELPORTTRACE ]  ") ; 
PARALLELPORTTRACE  =  TRUE; 


else 


if 


( { s  t  r cmp  ( keyword , 
(strcmp  (keyword, 
(strcmp  (keyword, 
(strcmp  (keyword, 


"TACTICAL" ) 
"TACTICAL-HOST") 
"TACTICALHOST" ) 
"STRATEGIC") 


==  0)  II 
==  0)  M 
==  0)  M 
==  0)  II 
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(strcmp  (keyword,  "STRATEGICHOST'' )  ==  0)  I  [ 

(strcmp  (keyword,  "STRATEGIC-HOST")  ==  0)) 

{ 

if  (sscanf  (command_buf fer,  "%s  %s",  keyword,  parameter^string)  ==  2) 

TACTICAL  =  TRUE; 

KEYBOARDINPUT  =  FALSE; 

strcpy  { tactical_remot:e_host_naiDe,  parameter_st:ring}  ; 
open_tact:ical_socket  ( )  ; 
if  (DISPLAYSCREEN) 

printf ("\n[%s  %s  (KEYBOARD-OFF)]", 

keyword,  tactical_remote_host_narae) ; 

) 

else  print_help  =  TRUE; 

} 

else  if  ((strcmp  (keyword,  "NO- TACTICAL")  ==  0) '  M 
(strcmp  (keyword,  " TACTICAL- OFF " )  ==  0)) 

{ 

if  (DISPLAYSCREEN)  printf  ("\n[%s]\n",  keyword); 

TACTICAL  =  FALSE; 

} 

else  if  ((strcmp  (keyword,  " SONAR INSTALLED " )  ==  0)  M 
(strcmp  (keyword,  " SONAR- INSTALLED" )  ==  0)) 

{ 

if  (DISPLAYSCREEN)  printf  (" \n [SONAR- INSTALLED]  "); 
SONARINSTALLED  =  TRUE; 

} 

else  if  ((strcmp  (keyword,  "NOSONAR INSTALLED" )  ==  0)  li 

(strcmp  (keyword,  "NO-SONAR- INSTALLED" )  ==  0)) 

{ 

if  (TRACE  ScSc  DISPLAYSCREEN)  printf  ("  [NO- SONAR -INSTALLED]  "); 
SONARINSTALLED  =  FALSE; 

} 

else  if  ((strcmp  (keyword,  "SONARTRACE" )  ==  0)  11 

(strcmp  (keyword,  " SONAR- TRACE" )  ==  0)  i! 

(strcmp  (keyword,  " SONAR -TRACE- ON " )  ==  0)) 

{ 

if  (DISPLAYSCREEN)  printf  (" \n [SONARTRACE]  "); 

SONARTRACE  =  TRUE; 

} 

else  if  ((strcmp  (keyword,  "SONARTRACEOFF" )  ==  0)  II 

(strcmp  (keyword,  " SONAR- TRACE-OFF" )  ==  0)) 

{ 

if  (DISPLAYSCREEN)  printf  ("\n [SONARTRACEOFF]  ") ; 

SONARTRACE  =  FALSE; 

} 

else  if  (strcmp  (keyword,  " PARALLEL PORTTRACE " )  ==  0) 

{ 

if  (DISPLAYSCREEN)  printf  ("\n [PARALLELPORTTRACE]  "); 


PARALLELPORTTRACE  = 

} 

else  if  ((strcmp  (keyword. 

TRUE; 

"AUDIBLE") 

==  0) 

(strcmp 

(keyword. 

"AUDIO") 

==  0) 

(strcmp 

(keyword. 

"AUDIO-ON") 

==  0) 

(strcmp 

(keyword. 

"SOUND-ON" ) 

==  0) 

(strcmp 

(keyword. 

"SOUNDON" ) 

==  0) 

(strcmp 

(keyword. 

"SOUND") 

==  0)) 

{ 

if  (DISPLAYSCREEN)  printf  (" \n [AUDIBLE]  ") ; 
strcpy  (buffer,  "AUDIBLE");  /*  copy  current  command  to  buffer  */ 
send_buf fer_to_virtual_world_socket  ();/*send  to  sound  driver  */ 


} 

else  if  ((strcmp  (keyword,  "SILENT")  ==  0) 

(strcmp  (keyword,  "SILENCE")  ==  0} 

(strcmp  (keyword,  "NOSOUND")  ==  0) 

(strcmp  (keyword,  "NO- SOUND")  ==  0) 

(strcmp  (keyword,  "SOUNDOFF")  ==  0) 

(strcmp  (keyword,  "SOUND-OFF")  ==  0) 
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(strcmp  (keyword,  "AUDIOOFF")  ==  0)  II 
(strcmp  (keyword,  "AUDIO-OFF")  ==  0)  11 
(strcmp  (keyword,  "QUIET")  ==  0)) 

{ 

if  (DISPLAYSCREEN)  printf  ("\n[SILENT]  "); 

strcpy  (buffer,  "SILENT");  /*  copy  current  command  to  buffer  */ 
send_buffer_to_virtual_world_socket  {);/*send  to  sound  driver  */ 

} 

#ifndef  OS9 

/*  save  space  due  to  OS9  size  problems  */ 

else  if  ((strcmp  (keyword,  " SOUNDS ER lAL " )  ==  0)  M 

(strcmp  (keyword,  " SOUND- SERIAL" )  ==  0)) 

{ 

if  (TRACE  ScSc  DISPLAYSCREEN)  printf  { "\n  [SOUNDSERIAL  ON]  "); 
strcpy  (buffer,  "SOUNDSERIAL");  /*  send  precise  keyword  */ 

send_buf f er_to_virtual_world_socket  ();/*send  to  sound  driver  */ 
audible_command  =  FALSE; 

} 

else  if  ((strcmp  (keyword,  " SOUND PARALLEL " )  ==  0)  M 

(strcmp  (keyword,  " SOUND- PARALLEL " )  ==  0)) 

{ 

strcpy  (buffer,  "SOUNDPARALLEL" ) ;  /*  send  precise  keyword  */ 

send_buf fer__to__virtual_world_socket  ();  /*send  to  sound  driver*/ 
audible_command  =  FALSE; 

} 


else  if  ((strcmp 

(keyword, 

"EMAIL") 

==  0)  II 

(strcmp 

(keyword. 

"EMAIL-ON") 

==  0)  M 

(strcmp 

(keyword. 

"E-MAIL") 

==  0)  II 

(strcmp 

(keyword. 

"E-MAIL-ON") 

==  0 )  11 

(strcmp 

(keyword. 

"EMAILON") 

==  0)) 

if  (TRACE  &&  DISPLAYSCREEN)  printf  ("\n[EMAIL  ON]  ") ; 
EMAIL  =  TRUE; 

} 


else  if  ((strcmp 

(keyword. 

"EMAILOFF") 

0) 

(strcmp 

(keyword. 

"EMAIL-OFF") 

0) 

(strcmp 

(keyword. 

"E-MAILOFF") 

0) 

(strcmp 

(keyword, 

"E-MAIL-OFF") 

0) 

(strcmp 

(keyword. 

"NO-E-MAIL") 

== 

0) 

(strcmp 

(keyword. 

"NO-EMAIL") 

=r=r 

0) 

(strcmp 

(keyword. 

"NO-E-MAIL") 

=  = 

0) 

(strcmp 

(keyword. 

"NOEMAIL") 

=  = 

0)) 

if  (TRACE  ScSc  DISPLAYSCREEN)  printf  ("\n[EMAIL  OFF]  "); 

EMAIL  =  FALSE; 

} 

#endif 

else  if  {(strcmp  (keyword,  "WAYPOINT")  ==  O)  I  I 
(strcmp  (keyword,  "WAYPOINT-ON")  ==  0)) 

{ 

#ifndef  os9 

parameters_read  =  sscanf  (command_buf f er,  "%s%lf%lf%lf " , 

keyword,  &  parameterl, 

&  parameter2,  &  parameters); 

#else 


#endif 


parameters_read  =  sscanf  (command__buf fer ,  "%s%F%F%F", 

keyword,  &  parameterl, 

&  parameter2,  &  parameters); 

if  (parameter  s__read  ==  5) 

{ 

if  (DISPLAYSCREEN) 

printf  ("\n[%s  %6.2f  %6.2f  %6.2f  %6.21f]\n", 

keyword,  parameterl, 
parameters,  parameters, 
parameter 4 ) ; 

WAYPOINTCONTROL  =  TRUE; 

FOLLOWWAYPOINTMODE  =  TRUE; 
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HOVERCONTROL  =  FALSE; 

ROTATECONTROL  =  FALSE; 

LATERALCONTROL  =  FALSE; 

TARGETCONTROL  =  FALSE; 

RECOVER YCONTROL  =  FALSE; 

INTEGRALDEPTHCONTROL  =  FALSE; 

time_int_control_on  =  t  +  10.0;  /*  give  PD  10  seconds  */ 
SONARSCANMODE  =1;  /*  Forward  Scan  */ 

REPORTSTABLE  =  TRUE; 

DEADSTICKRUDDER  =  FALSE; 
x_command  =  parameter!; 

y^command  =  parameter 2 ; 

z_command  =  parameters; 

port_rpm_command  =  fabs  (parameter4) ;  /*  ensure  fwd  */ 

stbd__rpm_command  =  port_rpm_command;  /*  motion  only  */ 

DEATH_SPIRAL_RESET  =  TRUE; 

} 

else  if  (parameters_read  ==  4) 

{ 

if  (DISPLAYSCREEN) 

printf  ("\n[%s  %6.2f  %6.2f  %6.2f]\n", 

keyword,  parameterl , 
parameter2,  parameters); 

WAYPOINTCONTROL  =  TRUE; 

FOLLOWWAYPOINTMODE  =  TRUE; 

HOVERCONTROL  =  FALSE; 

ROTATECONTROL  =  FALSE ; 

LATERALCONTROL  =  FALSE; 

‘TARGETCONTROL  =  FALSE; 

RECOVER YCONTROL  =  FALSE; 

INTEGRALDEPTHCONTROL  =  FALSE; 

time_int_control_on  =  t  +  10.0;  /*  give  PD  10  seconds  */ 
SONARSCANMODE  =1;  /*  Forward  Scan  */ 

REPORTSTABLE  =  TRUE; 

DEADSTICKRUDDER  =  FALSE; 
x_command  =  parameterl; 

y_command  =  parameters,* 

z^command  =  parameters,* 

po3:t_rpm_command  =  fabs  (port_rpm_command)  ,*  /*  ensure  fwd  */ 

stbd_rpm_command  =  fabs  {stbd_rpm_coinmand)  ,*  /*  motion  only  */ 

DEATH_SPIRAL_RESET  =  TRUE; 

.} 

else  if  (parameters_read  ==  3) 

{ 

if  (DISPLAYSCREEN) 

printf  ("\n[%s  %6.2f  %6.2f]\n",  keyword,  parameterl, 

parameters); 

WAYPOINTCONTROL  =  TRUE; 

FOLLOWWAYPOINTMODE  =  TRUE; 

HOVERCONTROL  =  FALSE; 

ROTATECONTROL  =  FALSE; 

LATERALCONTROL  =  FALSE; 

TARGETCONTROL  =  FALSE ; 

RECOVERYCONTROL  =  FALSE; 

INTEGRALDEPTHCONTROL  =  FALSE; 

time_int_control_on  =  t  +  10,0;  /*  give  PD  10  seconds  */ 
SONARSCANMODE  =1;  /*  Forward  Scan  */ 

REPORTSTABLE  =  TRUE; 

DEADSTICKRUDDER  =  FALSE; 
x_command  =  parameterl ; 

y_command  =  parameters,* 

port_rpm_command  =  fabs  (port_rpm_command)  ;  /*  ensure  fwd  */ 

stbd_rpm_command  =  fabs  (stbd_rpm_command) ;  /*  motion  only  */ 

DEATH_SPIRAL_RESET  =  TRUE; 
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else 

{ 

WAYPOINTCONTROL  =  FALSE; 
if  (DISPLAYSCREEN) 

{ 

printf  ("\n  warning:  improper  number  of  valuesXn  waypoint"); 
printf  ("set  to  current  position  but  otherwise  ignoredXn"); 
} 

x_command  =  x; 

y_command  =  y; 

z_command  =  z; 

} 

i f  { FOLLOWWAYPO INTMODE ) 

{ 

/*  continue  until  WAYPOINT  reached  without  further  script  orders  */ 
time_next_command  =t+dt; 
read_another_line  =  FALSE; 

} 

} 

else  if  ({strcmp  (keyword,  "WAYPOINTFOLLOW")  ==  0)  I! 

(strcmp  (keyword,  "WAYPOINT-FOLLOW" )  ==  0)  I! 

(strcmp  (keyword,  "WAYPOINTFOLLOWON" )  ==  0)  II 

(strcmp  (keyword,  "WAYPOINT- FOLLOW-ON" )  ==  0)) 

{ 

if  (DISPLAYSCREEN)  printf  ("\n[%s]Xn",  keyword); 
FOLLOWWAYPOINTMODE  =  TRUE; 

DEADSTICKRUDDER  =  FALSE; 

} 

else  if  ({strcmp  (keyword,  "WAYPOINTFOLLOWOFF" )  ==  0)  li 

(strcmp  (keyword,  "WAYPOINT- FOLLOW- OFF")  ==  0)  ) 

{ 

if  (DISPLAYSCREEN)  printf  {"Xn[%s]Xn",  keyword); 
FOLLOWWAYPOINTMODE  =  FALSE; 

} 

else  if  ((strcmp  (keyword,  "TARGET- OFF " )  ==  0)  I  I 

(strcmp  (keyword,  "TARGETOFF")  ==  0)  II 

(strcmp  (keyword,  "NO-TARGET")  ==  0)  M 
(strcmp  (keyword,  "NOTARGET")  ==  0)) 

{ 

TARGETCONTROL  =  FALSE; 

RECOVERYCONTROL  =  FALSE; 

SONARSCANMODE  =1;  /*  Forward  Scan  */ 

target_bearing_command  =  0.0; 
ta  rget_range_coTnmand  =  0.0; 

} 

else  if  ({strcmp  (keyword,  "TARGET- POINT" )  ==0)  M 

(strcmp  (keyword,  "TARGETPOINT")  ==  0)) 

{ 

TARGETPOINTING  =  TRUE; 

} 

else  if  ((strcmp  (keyword,  "NO- TARGET- POINT")  ==  0)  II 
(strcmp  (keyword,  "NOTARGETPOINT")  ==  0)  I  I 

(strcmp  (keyword,  "TARGET- POINT- OFF" )  ==0)  II 
(strcmp  (keyword,  "TARGETPOINTOFF" )  ==  0)) 

{ 

TARGETPOINTING  =  FALSE; 

} 

else  if  ({strcmp  (keyword,  "STANDOFF")  ==  0)  M 

(strcmp  (keyword,  "STAND-OFF")  ==  0)  I  I 

(strcmp  (keyword,  "STANDOFFD I STANCE")  ==  0)  II 

(strcmp  (keyword,  "STANDOFF -DISTANCE")  ==  0)  I  I 

(strcmp  (keyword,  "STAND-OFF-DISTANCE")  ==  0)) 

{ 

#ifndef  os9 

parameters_read  =  sscanf  (command_buf f er,  "%s%lf". 
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#else 


keyword. 


Sc  parameterl )  ; 


#endif 


} 

else 

{ 


} 

else 

{ 

) 

else 

{ 

} 

else  i 
{ 

fifndef  os9 

#else 

#endi  f 


parameters_read  =  sscanf  (coininand_buf fer,  "%s%F", 

keyword,  &  parameterl) ; 


if  (parameters^read  ==  2) 

{ 

if  (DISPLAYSCREEN)  printf  (nn[%s  %6.2f]\n", 
keyword,  parameterl); 
standoff_di stance  =  parameterl; 

if  (HOVERCONTROL)  /*  report  when  stable  again  */ 
REPORTSTABLE  =  TRUE; 

} 

else 

{ 

if  (DISPLAYSCREEN) 

{ 

printf  ("\n[%s]\n",  keyword); 
printf  ("  warning:  no  standoff  value  provided,  ignored''); 

} 

} 

if  ((strcmp  (keyword,  "HOVEROFF")  ==  0)  li 

(strcmp  (keyword,  "HOVER-OFF")  ==  0)  II 

(strcmp  (keyword,  "HOVER_OFF")  ==  0)) 

if  (DISPLAYSCREEN)  printf  {" \n [HOVER-OFF]  "); 

HOVERCONTROL  =  FALSE; 

WAYPOINTCONTROL  =  FALSE; / *explici tly  eliminate  side  effects  */ 
FOLLOWWAYPOINTMODE  =  FALSE; 

port_rpm_command  =  0.0; 

stbd_rpm_command  =  0.0; 

rudder_coinmand  =  0.0; 

read_another_line  =  FALSE; 

if  ((strcmp  (keyword,  "TEXT")  ==  0)  II 

(strcmp  (keyword,  "TEXT-ON")  ==  0)) 

DISPLAYSCREEN  =  TRUE; 

if  ((strcmp  (keyword,  "NOTEXT")  ==  0)  II 
(strcmp  (keyword,  "NO-TEXT")  ==  0)) 

DISPLAYSCREEN  =  FALSE; 

-f  {(strcmp  (keyword,  "SCANWIDTH")  ==  0)  il 
(strcmp  (keyword,  "SCAN-WIDTH")  ==  0) ) 


parameters_read  =  sscanf  (command_buffer,  "%s%lf", 

keyword,  &  parameterl) ; 

parameters_read  =  sscanf  (command_buf fer,  "%s%F", 

keyword,  &  parameterl); 


if  (parameter spread  ==  2) 

{ 

if  ((parameterl  <=  0.0)  &&  DISPLAYSCREEN) 

{ 

printf ("  illegal  SCANWIDTH  value,  ignored."); 
printf("  [SCANWIDTH  =  %3.2lf]\n",  SCANWIDTH); 

} 

else 

{ 

SCANWIDTH  =  parameterl; 
if  (DISPLAYSCREEN) 

printf {" [SCANWIDTH  %3.21f]",  SCANWIDTH); 
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} 

} 

} 

else  /*  check  other  possibilities  */ 

{ 

^  parse_mission_string_coiTiinands  (command^buf  fer)  ; 

if  (audible^coimnand) 

{ 

strcpy  (buffer,  command^buffer) ;  /*  copy  current  command  to  buffer  */ 
send_buf fer_to_virtual_world_socket  ();  /*  send  to  sound  driver  */ 

if  ( (print_help)  &&  DISPLAYSCREEN) 

if  (DISPLAYSCREEN)  printf  ("%s",  command^buffer); 
print_valid_keywords  {); 

strcpy  (buffer,  "  is  an  unknown  command");  /*  copy  msg  to  buffer  */ 
send_buf f er_to__virtual__world_socket  ();  /*  send  to  sound  driver  */ 

} 

return_yalue  =  print_help; 

print_help  =  FALSE;  /*  reset  value  */ 

if  (TACTICAL) 

{ 

tiine__next_command  =  t  +  dt;  /*  one  command  per  timestep  only  */ 

^sad_another__line  =  FALSE;  /*  force  acknowledgement  and  loop*/ 

/*  TIME  and  WAIT  and  RUN  commands  are  not  needed  in  TACTICAL  mode  */ 

if  ( (HOVERCONTROL)  1}  (WAYPOINTCONTROL)  M  (REPORTSTABLE) ) 

read_another_line  =  FALSE;  /*  force  acknowledgement  and  loop*/ 

/*  TIME  and  WAIT  and  RUN  commands  are  not  needed  in  TACTICAL  mode  */ 

} 

}  /*  loop  until  read_another_line  is  FALSE)  */ 
if  (TRACE  ScSc  DISPLAYSCREEN) 

printf  ("\n[end  parse_mission_script_commands  {)]\n"); 
return  (return_value) ; 

}  /*  end  parse_mission_script_commands  ()  */ 

/***************★****************★******************************* ****^***^*^^ 
void  parse_mission_string_commands  (command) 


char  * 

command; 

int 

index; 

int 

number_values  =  0; 

char 

parameter_string  [60] ; 

if  (NOSCRIPT)  return;  /*  no  script,  telemetry  playback  only  */ 

if  (TRACE  &&  DISPLAYSCREEN) 

printf  ( "\n [parse_mission_string_commands  start] \n"); 

number_values  =  sscanf  (command_buf fer,  "%s",  keyword); 

for  (index  =  0;  index  <=  (int)  strlen  (keyword);  index++) 
keyword  [index]  =  toupper  (keyword  [index]); 

if  ( numb e revalues  !=  1) 

{ 
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if  (DISPLAYSCREEN)  printf  ("  [no  parse  word  found]  Xn^'j^- 
return; 


( (strcmp 

(keyword. 

"VIRTUALHOST" ) 

0) 

(strcmp 

(keyword. 

"VIRTUAL") 

== 

0) 

(strcmp 

(keyword. 

"VIRTUAL-HOST" ) 

== 

0) 

(strcmp 

(keyword. 

"REMOTE" ) 

== 

0) 

(strcmp 

(keyword. 

"REMOTEHOST" ) 

== 

0) 

(strcmp 

(keyword. 

"REMOTE-HOST") 

== 

0) 

(strcmp 

(keyword. 

"DYNAMICS"  ) 

== 

0)) 

{ 

if  (sscanf  (command,  "%s  %s",  keyword,  parameter_string)  ==  2) 

{ 

strcpy  {virtual_world_remote_host_name,  parameter_string)  ; 
if  (DISPLAYSCREEN)  printf  ("\n [VIRTUAL-HOST  %s]  ", 

virtual_world_remote_host__naine)  ; 

} 

else  print_help  =  TRUE; 

} 

else  print_help  =  TRUE;  /*  invalid  command  line  entry  parameter  found  */ 
if  (TRACE  5c&  DISPLAYSCREEN) 

printf  ("\n[parse_mission_string_commands  complete] \n") ; 
return; 


}/*  end  parse_mission_string_commands  ()  */ 

/***************************************************************************/ 

void  print_valid_keywords  () 

{ 

if  (TACTICAL  1  I  TACT I CAL PARSE)  return; 


printf 

printf 

printf 

printf 

printf 

printf 

printf 

printf 

printf 

printf 

printf 

printf 

printf 

printf 

printf 


("\n") 

{" 

i" 

{" 

{" 

{" 

(" 

(" 

r 

{" 

{" 

r 

r 

("See  - 
("\n")  ; 


[help]  [trace  I  notrace]  [loopf orever ! looponce] \n") ; 
[wait  #]  [time  #]  [timestep  (0.0.. 5.0)]  [mission] \n") ; 

[keyboard  I  keyboard-off]  [quit]  [kill]\n") ; 

[rpm]  [course]  [depth]  [thrusters j thrusters-off] \n") ; 
[ loopf ilebackup]  [entercontrolconstants] \n") ; 
[rotate]  [position  I  location  I  fix]  [orientation] \n" ) ; 
[gps I gps-f ix]  [gps-complete I gps-fix- complete]  \n" ) ; 
[sonartrace I sonartraceoff ]  [sonarinstalled] \n") ; 
[trace I  trace-off]  [parallelporttrace]  \n") ; 
[remotehost  hostname] [realtime  I  nopause]  [pause] \n"); 
[ loop-forever  I loop-once] [controlconstantsf ile] \n" ) ; 
[silence] [e-mail  I  no-email]  [waypoint] \n\n" ) ; 
/execution/mission. script .HELP  for  command  syntax  details .\n"); 


#ifdef  sgi 

/*  don't  pop  up  help  file  if  TACTICAL  is  running  or  invoking  this  code  */ 
if  ( (HELPFILELAUNCHED  ==  FALSE)  &&  (TACTICAL  ==  FALSE)  && 

(TACTICALPARSE  ==  FALSE)) 

{ 

printf  ("popping  up  'mission. script .HELP'  as  a  zip  file...\n"); 
system  ("zip  -v  -/execution/mission . script . HELP" ) ; 

HELPFILELAUNCHED  =  TRUE; 

} 

#endif 


return; 

}  /*  end  print_valid_keywords  */ 
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void  get_control_constants  {) 

^  /*  get  data  from  file  at  program  start  */ 

if  (TACTICALPARSE)  return; 

if  (TRACE  &&  DISPLAYSCREEN) 

printf  ("\n[start  get_control_constants  ()l\n"); 

if  ( ENTERCONTROLCONSTANTS  &&  DISPLAYSCREEN) 

{ 

printf  ("Error:  ENTERCONTROLCONSTANTS  not  possible  while  "); 
printf  ("DISPLAYSCREEN  is  FALSE. \n"); 

printf  ("Using  originally  loaded  control  constants . \n" ) ; 
ENTERCONTROLCONSTANTS  =  FALSE; 

} 


else  if  (ENTERCONTROLCONSTANTS)  /*- - - - -*/ 

{ 

printf ("Input  start_dwell  (startup  delay  time  in  seconds) \n" ) ; 
scanf("%d",  &start_dwell ) ; 

/*  note  %F  required  by  OS-9,  accepted  by  SGI  as  equivalent  to  %lf  */ 


printf ("Input  k_psi,  k_r.  k_v\n"); 

#ifndef  os9 

scanf("%lf  %lf  %lf",  &k_psi.  &k_r.  &k_v) 

#else 

scanf("%F  %F  %F".  &k_psi,  &k_r.  &k_v) ; 

#endif 

printf ("Input  k_z,  k_w,  k_theta,  and  k_q\n"); 

#ifndef  os9 

scanf("%lf  %lf  %lf  %lf",  &k_z.  &k_w.  &k_theta,  &k_q) ; 

#else 

scanf("%F  %F  %F  %F".  &k_z.  &k_w.  &k_theta.  &k_q) ; 

#endif 

printf ("Input  k_thruster_psi .  k_thruster_r\n" ) ; 
tifndef  os9 

scanf("%lf  %lf",  &k_thruster  _iDsi ,  &k_thruster__r)  ; 

#else 

scanf("%F  %F".  &k_thruster_psi .  &k_thruster_r) ; 

#endif 

printf ( " Input  k_thruster_rotate\n" ) ; 

#ifndef  os9 

scanf ("%lf".  &k_thruster_rotate) ; 

#else 

scanf ( "%F" ,  &k_thruster_rotate) ; 

#endif 

printf ("Input  k_thruster_z,  k_thruster_w\n") ; 

#ifndef  os9 

scanf ("%lf  %lf".  &k_thruster_z.  &k_thruster_w) ; 

#else 

scanf ("%F  %F",  &k_thruster_z .  &k_thruster_w) ; 

#endif 

printf ( " Input  k_thruster_theta\n" ) ; 

#ifndef  os9 

scanf ("%lf",  &k_thruster_theta) ; 

#else 

scanf ("%F".  &k_thruster_theta) ; 

#endif 

printf ("Input  k_propell€r_hover.  k_surge_hover, 
k_^  rope ller_cur rent \n " ) ; 

#ifndef  os9 

scanf  ("%lf  %lf  %lf",  &k__propGller_hover.  &k_surge_hover. 

#else 
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scanf("%F  %F  %F^', 

#endif 


&]c_p  rope  lie  r_hove  r ,  &k_su  r  ge_hove  r , 
Sck _propeller__current) ; 


printf ("Input  k_thruster__hover,  k_sway_hover/  k_thruster_current\n") ; 
#ifndef  os9 

scanf("%lf  %lf  %lf",  &k_thruster_hover, 

#else 

scanf("%F  %F  %F",  &k_thruster_hover, 

#endif 

&k__sway_hover ,  &k_thrus ter_current ) ; 


printf ("Input  k_thruster_lateral\n" ) ; 

#ifndef  os9 

scanf ("%lf",  &k_thruster_lateral ) ; 

#else 

scanf ( "%F" ,  &k_thruster_lateral } ; 

#endi f 
) 

else  if  (LOADCONTROLCONSTANTS) 

{ 

if  ( (controlconstantsinputfile  =  fopen  (CONTROLCONSTANTSINPUT- 
NAME, "r") ) 

==  NULL) 

{ 

printf ("AUV  execution:  unable  to  open  control  constants  input  file  "); 
printf ("%s  for  reading. \n",  CONTROLCONSTANTSINPUTNAME) ; 
printf 

("  Check  ownership  permissions  in  current  directoryAn"); 

printf ("Exit . \n") ; 
exit  (-1); 

} 


strcpy  (buffer,  "Control  constants  file  is"); 

if  (TRACE  ScSc  DISPLAYSCREEN) 

{ 

printf  ("\n [controlconstantsinputfile  %s  open,  pointer  =  %x]\n", 
CONTROLCONSTANTSINPUTNAME,  controlconstantsinputfile) ; 
send_buf fer_to„virtual_world_socket  ();  /*  buffer  message  sent  */ 

} 

strcpy  (buffer,  CONTROLCONSTANTSINPUTNAME); 
if  (TRACE  &&  DISPLAYSCREEN) 

{ 

send_buf fer__to_virtual_world_socket  ();  /*  buffer  message  sent  */ 

} 

start_dwell  =1;  /*  delay  time  in  seconds  */ 

/*  skip  remaining  header  lines  in  file  */ 

for  (i=l ; i<=8; i++)  fgets  ( local_buf f er,  80,  controlconscantsinputf i le) ; 

/*  note  %F  required  by  OS-9,  accepted  by  SGI  as  equivalent  to  %lf  */ 
#ifndef  os9 

fscanf  (controlconstantsinputfile,  "%lf  %lf  %lf",  Sckjsi,  &k_r,  &k_v)  ; 

#else 

fscanf  (controlconstantsinputfile,  "%F  %F  %F",  6ck_psi,  &k_r,  &k_v) ; 

#endif 


#ifndef  os9 

fscanf 

#else 


fscanf 

#endif 


(controlconstantsinputfile, 

(controlconstantsinputfile. 


"%lf  %lf  %lf  %lf", 
"%F  %F  %F  %F", 


Sck_2,  fick^w, 

&k_z,  &k_w. 


&k_theta,  &k_q) ; 

for  (i=l;i<=5;i++)  fgets  (local_buf fer ,  80,  controlconstantsinputfile); 
#ifndef  os9 
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#else 


#Gndif 


f scanf  (controlconstantsinputfile,  "%lf  %lf'‘ 
fscanf (controlconstantsinputfile,  "%F  %?" , 


#ifndef  os9 

fscanf (controlconstantsinputfilG,  "%lf " , 


#else 

#endif 


fscanf (controlconstantsinputfile,  "%F" , 


&k_thrustGr_psi , 
&k_thruster_psi , 
Sck_thruster_r ) ; 

£ck_thruster__rotate}  ; 
&k_thruster_rotate) ; 


for  {i=l;i<=5;i++)  fgets  (local_buf f er,  80,  controlconstantsinputfile) ; 
#ifndef  os9 


#Glse 

#endif 


fscanf (controlconstantsinputfile,  "%lf  %lf" 
fscanf  (controlconstantsinputfile,  "■%?  %F", 


#ifndef  os9 

fscanf (controlconstantsinputfile,  "%lf", 

#else 

fscanf (controlconstantsinputfile,  "%F" , 

#endif 


&k_thruster_z , 
&k_thruster__z , 
&k_thruster__w)  ; 
&:k_thruster_theta)  ; 
&k_thruster_theta) ; 


for  (i=l;i<=5; i-}-+)  fgets  (local__buf f er,  80,  controlconstantsinputfile); 
#ifndef  os9 

fscanf (controlconstantsinputfile,  "%lf  %lf  %lf",  &k_propeller_hover, 
fscanf (controlconstantsinputfile,  "%F  %F  %F",  &k_propeller_hover, 


#else 

#endif 


& k_su rge_ho ve r , 
&k_propeller_current) ; 


for  (i=l ; i<=5 ; i++)  fgets  ( local_buf fer ,  80,  controlconstantsinputfile); 
#ifndef  os9 

fscanf (controlconstantsinputfile,  "%lf  %lf  %lf",  £ck_thruster_hover , 

fscanf (controlconstantsinputfile,  "%F  %F  %F",  &k_thruster_hover , 

Sck_sway_hover , 
&k_thruster_current) ; 


#else 

#endif 


for  (i=l ;i<=5; i++)  fgets  ( local_buf f er,  80,  controlconstantsinputfile); 
#ifndef  os9 


#else 


fscanf (controlconstantsinputfile,  "%lf ", 
fscanf (controlconstantsinputfile,  "%F", 


&k_thruster_lateral) ; 
&k_thruster_lateral) ; 


#endif 
} 

else  /*  use  default  initialization  values  - 
if  (TRACE  &&  DISPLAYSCREEN) 

printf  ("\n[using  default  control  constant  values] \n"); 


start_dwell  =  1; 


/*  delay  time  in  seconds  */ 


k_psi 

k_r 

k_v 

k_z 

k_w 

k_theta 


1.00;  /*  degrees  rudder  per  degree  of  course  error  */ 
2.00;  /*  degrees  rudder  per  degree/sec  yaw  rate  */ 
0.00;  /*  needed  ??  */ 

15.0;  /*  degrees  planes  per  foot  of  depth  error  */ 

2.0; 

4.0; 

1.0; 


rpm 


=  400.0; 
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k_thruster_psi  =  0.6;  /*  volts  per  1  degree  course  error  */ 

k_thruster_r  =  5.0; 

k_thruster_rotate  =  1.5;  /*(24V)^2=>  2  #  =  16.0  deg/sec  empirical*/ 

/*  k_thruster_rotate=  (24V  /  16  deg/ sec )  ^^2*/ 
k_thruster__z  =  20.0;  /*  guess  20  fresh  water,  30  in  sea  water  */ 

k_thruster_w  =  80.0; 

k_thruster_theta  =1.0; 


k_propeller_hover  =  200.0;  /*  200  rpmperone  foot  error  */ 
k__surge_hover  =  6000.0;/*  60  rpm  per  0 . 01  foot /sec  surge  */ 
/*  this  value  is  high  to  reduce  sternway  */ 
k_propeller_current  =  6500.0;  /*  experimental  */ 

k_thruster_hover  =  4.0; 
k_sway_hover  =  40.0; 

k_thruster_current=  40.0;  /*  experimental  */ 


k_thruster_lateral= 


} 


48.0;  /*  24  V  =  2  #  =  0.5  ft/sec  empirically  */ 

/*  note  voltage  follows  a  square  law  */ 


if  {DISPLAYSCREEN  && 

(ENTERCONTROLCONSTANTS  II  LOADCONTROLCONSTANTS  II  SHOWCONTROLCONSTANTS ) ) 

{ 

print f  ("\n"); 

printf  {"\n[kjpsi  =  %5.2f,  k_r  =  %5.2f,  k_v  =  %5.2f,  k_z  =  %5.2f,  ", 
k_psi,  k_r,  k_v,  k_z) ; 

printf  ("k_w  =  %5.2f,  k__theta  =  %5.2f,  k_q  =  %5.2f]\n", 
k_w,  k_theta,  k_q)  ; 

printf  ("\n[k_thruster_psi  =  %5.2f,  k_thruster_r  =  %5.2f,  ", 
k_thruster_psi,  k_thruster_r) ; 

printf  (  "k_thruster_rotate  =  %5.2f]\n", 

k_thruster_rotate) ; 

printf  {  " [k_thruster_z  =  %5.2f,  k_thruster_w  =  %5.2f,  ", 

k__thruster_z,  k_thruster_w)  ; 

printf  (  "k_thruster_theta  =  %5.2f]\n", 

k_thruster_theta) ; 

printf  ("\n[k_propeller_hover  =  %5.2f,  k_surge_hover  =  %5.2f]\n", 
k_propeller_hover,  k_surge_hover) ; 

printf  ("\n [k_propeller_current  =  %5.2f]\n", 
k_p rope ller_cur rent) ; 

printf  ("\n[k_thruster_hover  =  %5.2f,  k_sway_hover  =  %5.2f]\n'\ 
k_thruster_hover,  k_sway_hover ) ; 

printf  (^'\n[k__thruster_current  =  %5.2f]\n", 
k_thruster_current) ; 

printf  ("\n(k_thruster_lateral  =  %5 . 2f ] \n\n" , k_thruster_lateral ) ; 

} 


if  (SHOWCONTROLCONSTANTS  ==  TRUE) 

{ 

SHOWCONTROLCONSTANTS  =  FALSE; 
if  (TRACE  ScSc  DISPLAYSCREEN) 

printf  ("\n[finish  get_control_constants  ()]\n"); 
return; 

} 


if  ( (controlconstantsoutputfile  =  fopen  (CONTROLCONSTANTSOUTPUTNAME, "w" ) ) 
==  NULL) 

{ 

printf ("AUV  execution:  unable  to  open  control  constants  output  file  "); 
printf ("%s  for  writing. \n",  CONTROLCONSTANTSOUTPUTNAME); 
printf 
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("  Check  ownership  permissions  in  current  directory . \n" ) ; 

print f ( "Exit . \n" ) ; 
exit  (-1); 

} 

if  (TRACE  &&  DISPLAYSCREEN) 

printf  {"\n[controlconstantsoutputfile  %s  open,  pointer  =  %x]\n", 
CONTROLCONSTANTSOUTPUTNAME ,  control cons tant soutpu t f i 1 e ) ; 


/*  warning:  the  file  read  capability  depends  on  file  format/line  spacing  */ 
f printf  ( cont rolconstant sou tput file. 


fprintf 

fprintf 


"  _  \n\n"); 

( cont rolconstantsou tput file, 

"  AUV  execution  level  control  algorithm  coefficients  \n"); 

( controlconstantsou tput file. 


fprintf  (cont rolconstantsou tput file, 

"  k_psi  k_r  k_y  k_z  k_w 
fprintf  (cont rolconstantsou tput file, 

"  %5.2f  %5.2f  %5.2f  %5.2f  %5.2f 

k_psi,  k_r,  k_v,  k_z,  k_w. 


_  \n\n\n"); 

k_theta  k_q\n\n" ) ; 

%5.2f  %5.2f\n\n\n\n", 

k_theta,  k_q 


fprintf  (controlconstantsou tput file, 

"  k_thruster_psi  k_thruster_r 

fprintf  (cont rolconstantsou tput file, 

"  %5.2f  %5.2f 

k_thruster_psi,  k_thruster_r. 


k_thruster_rotate\n\n")  ; 

%5 .2f\n\n\n\n", 
k_thruster_rotate)  ; 


fprintf  (cont rolconstant sou tput file, 

"  k_thruster_z  k_thruster_w 

fprintf  ( cont rolconstantsoutput file, 

"  %5.2f  %5.2f 

k_thruster_z,  k_thruster_w, 


k_thruster_theta\n\n" ) ; 

%5.2f  \n\n\n\n", 
k_thruster_theta)  ; 


fprintf  (cont rolconstantsoutput file, 

"  k_propeller_hover  k_surge_hover  k_propeller_current\n\n" ) ; 
fprintf  (controlconstantsou tput file, 

"  %5.2f  %5.2f  %5.2f\n\n\n\n", 

k_propeller_hover,  k_surge_hover,  kjropeller_current)  ; 


fprintf  (controlconstantsou tput file, 

"  k_thruster_hover  k_sway_hover  k_thruster_current\n\n" ) ; 
fprintf  (cont rolconstantsou tput file, 

"  ,  %5.2f  %5.2f  %5.2f\n\n\n\n", 

.  k_thruster_hover,  k_sway_hover,  k_thruster_current) ; 

fprintf  (cont rolconstantsou tput file, 

"  k_thruster_lateral  \n\n"); 
fprintf  (cont rolconstantsoutput file, 

"  %5 .2f \n\n\n\n" , 

k_thruster_laterai) ; 


fflush  (controlconstantsoutputf ile) ;  /*  force  completion  of  file  write  */ 
fclose  (controlconstantsoutputf ile) ; 

if  (TRACE  &&  DISPLAYSCREEN) 

printf  ("\n[finish  get_control_constants  ()]\n"); 

return; 


}  /*  end  get_control_constants  ()  */ 

/*★************************•************************************************* ^ 


/*  end  parse_functions . c  */ 
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/*★***★★**************************************************★******★********** y 
/* 

Program:  execution,  c 

Description:  AUV  execution  level  program 

Authors:  Don  Brutzman,  Mike  Burns,  Duane  Davis, 

Dave  Marco  &  Walt  Landaker 

Revised:  2  August  96 

System:  AUV  Gespac  68020/68030,  OS-9  version  2.4 

Compiler:  Gespac  cc  Kernighan  &  Richie  (K&R)  C  (NOT  ANSI  C! ) 

Compilation:  ftp>  put  execution. c 

auvsiml>  chd  execution 
[68020]  auvsiml>  make  -k2f  execution 

[68030]  auvsiml>  make  execution 

[Irix  ]  -brutzman/execution»  make  execution 

Execution: 

[Irix  ]  ~brutzman/execution»  cd  execution 

'-brutzman/execution»  execution  remote  dynamics-hostname 

where  dynamics-hostname  is  the  IP  name  of  the  host  running 
the  dynamics  (virtual  world)  program 

Plotting:  see  gnuplot  scripts  'auv_plot .gnu'  &  'auv_j)lot_l_second.gnu' 

-brutzman/execution»  gnuplot  auv_plot .gnu 

Debugging:  -brutzman/execution»  lint  -Im  execution. c 

lint  -Im  -Iglobals.h  -Idefines.h  globals.c  parse_functions . c  \ 
execution .c 

-brutzman/execution»  make  warnings 

Description:  closed  loop  for  operation  during  vehicle  in-water 

missions  as  well  as  in  virtual  world 


Active  changes:  Don  Brutzman  working  lab/virtual  world  networked  version 

&  sonar  tracking  and  target  station  control 

Future  work:  Sonar /altimeter  integration  code  reintegrated/retested 

Audios  seem  to  be  generated  differently  by  OS-9 
standardize  parsing  of  command  line  and  script  commands 
fix  pitch  control 
finish  sliding  mode  control 


Testing  interprocessor  connections: 

parallel  port  /P '  OS-9  auvsiml>  mfi_a3 

LPTl :  DOS  auvsim2>  portfix 

>  print  filename.txt 

serial  port  (/Tl)  /TT  OS-9  auvsiml>  wr2tl  then  write  text 

OS-9  auvsiml>  rdtla  then  read  text 

DOS  auvsim2>  C:\COMM\PROCOMM 

then  <alto>  for  chat  mode 
<altF10>  help,  <altX>  exit 
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Interfaces : 

Telemetry  sent  via  serial  port  /tt  [==  /tl  at  high  baud  rate] 

Telemetry  received  via  parallel  port  /P 

Telemetry  is  optionally  passed  to/from  tactical  level  running  on  80386 

Reads  files:  mission. init  [mission  initialization  data  file  ] 

Writes  files:  output. data  [vehicle  telemetry  state  vector  data] 

output. auv  [tactical  order /executive  report  log] 

Sonar  commands /replies  via  device  port  /t3 

Note  that  %F  double  formats  are  used  instead  of  %lf  on  scanf ()  and  sscanf () 
calls  for  OS“9  compatibility.  SGI  C  compiler  does  not  complain, 
gcc  on  Sun  does  fail  at  run-time,  so  ifdef's  are  used  to  support 
the  proper  format  string. 


*/ 


*******  j 


tinclude  "globals.h" 
tinclude  "statevector .h" 
#include  "defines. h" 


/*************************************************************** *******^*^^.^^ 
/*  function  prototypes  */ 


/* 

*/ 

/* 

*/ 


is  there  some  way  to  put  parameter  specifications  in  the  prototypes?? 

only  if  we  buy  the  ANSI  C  compiler  from  Microware  (or  shift  to  VxWorks) 


/*  thus  following  prototypes  are  missing  parameters  : ( 


'^oid  closed_loop_control_module  (); 

double  read_depth  {); 
double  read_psi  0; 
double  read_roll__rate_gyro  (); 
double  read_pitch_rate_gyro  (); 
double  read_yaw_rate_gyro  (); 
double  read_port_motor_rpm  {); 
double  read_stbd_inotor_rpm  (); 
double  read_motor  O; 
double  read_roll_angle  0; 
double  2read_pitch_angle  0; 
double  read_heading  ( )  ; 
double  read__speed  (); 

void  kalman_z  0; 
void  'kalman_sonarl000  ()| 
void  kalman_sonar725  0  ; 

void  XY_psi_model_est  (); 

double  read__computer_battery_vol  tage  (); 
double  read_motor_gyro_battery_voltage  (); 
void  XY_jnodel_est  (); 
int  leak_check  (); 

void  2ero__qyro  data  (); 
void  2ero_surfaces  {); 
void  initialize__adcs  (); 
void  init_pia  {); 
void  init_timla  (); 
void  thruster^power  {); 


*/ 
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void 

screw_power 

0  ; 

void 

conunand_control_surface 

{)  ; 

void 

c  ominand^r  udde  r 

{)  ; 

void 

c  omma  nd_p  1  a  ne  s 

0  ; 

void 

command_jprope  1 1  or  s_o  f  f 

{)  ; 

void 

coiTimand_thrus  t  ers_o  f  f 

0  ; 

void 

c  oininand_jnot  or 

{)  ; 

void 

test_alive 

{)  ; 

void 

get_ini t_avg 

0  ; 

void 

get_avg_rng 

0; 

void 

open_device_paths 

{)  ; 

void 

tt:y_jnode 

0  ; 

void 

close_device_paths 

0  ; 

void 

read_para  Heliport 

0; 

unsigned  char 

read_timlacl 

0  ; 

void 

write^timla 

0; 

void 

send_dacl 

0  ; 

void 

send_dac2b 

0  ; 

int 

get_adcl 

0; 

int 

get_adc2 

0; 

void 

Init__PortA 

{)  ; 

void 

Init_PortB 

0  ; 

unsigned 

char 

Read_PortA 

0  ; 

unsigned  char 

Read_PortB 

0  ; 

unsigned  short 

Read_PortAB 

0  ; 

void 

set_bsyA 

0  ; 

void 

rst_bsyA 

0  ; 

int 

ck_sta 

0  ; 

void 

initialize^sonar 

{)  ; 

char 

set_scanninq  .gain 

0  ; 

char 

send_sonar_command 

{)  ; 

void 

center_sonar 

{)  ; 

void 

set_step_size 

0  ; 

void 

step_sonar 

0  ; 

void 

ping_sonar 

0  ; 

double 

read^sonar 

0  ; 

void 

control__sonar 

{); 

void 

open_virtual_world_socket 

0  ; 

void 

s  hu  t down_vi r tua l_wo  r 1  d_so  c  ke  t 

0  ; 

void 

s  end_bu  f  f  e  r_t  o_vi r t u a l_wo  r 1 d_s o c k e  t 

0; 

void 

get_string_f rom_virtual_world_socket 

{}; 

void 

record_data 

0; 

void 

execute_shutdown_script 

0; 

void 

c  ompu  t  e_h  o  ve  r_c  on  t  r  o  1  s 

0; 

void 

c  ompu  t  e_t  arge  t_cont  r  o  1  s 

{); 

void 

c  ompu  te__r  e  c  o  ve  ry_c  ont  ro  1  s 

0; 

void 

compute_waypoint_controls 

0; 

void 

compute_lateral_controls 

0; 

void 

compute_rotate_controls 

0  ; 

void 

compute_lateral_thrusters 

0; 

void 

compute_vertical_thrusters 

0  ; 

void 

compute_f  in_controls 

0; 

Functions 

added 

(  to  implement  Dave  M's  speed  control 

*/ 

int 

port_speed_control 

0  ; 

int 

s  tbd__speed_cont  ro  1 

0; 
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/*  Dive  Tracker  Functions  */ 


int 

createdmod 

{)  ; 

int 

CLReaddmod 

0  ; 

void  * 

AttachMod 

0  ; 

int 

DettachMod 

{) ; 

void  * 

CreateMod 

0; 

/**************-k*ick-kifkieicic-k-kic*’k**icicic*ic-kicici(***ic*-kic*'k-kific*ic-kic**-k-kic**ic-k-kic-k*icicic 

/*  external  function  prototypes 
/*  from  external_functions.c 


extern 

double 

degrees 

0  ; 

extern 

double 

radians 

{)  ; 

extern 

double 

normalize 

{)  ; 

extern 

double 

normalize2 

0  ; 

extern 

double 

radian_normalize 

0  ; 

extern 

double 

radian_normalize2  ‘ 

0  ; 

extern 

void 

clamp 

0  ; 

extern 

double 

atan2 

0  ; 

extern 

double 

atan2z 

0  ; 

extern 

double 

sinh 

0  ; 

extern 

double 

cosh 

0  ; 

extern 

double 

tanh 

0  ; 

extern 

void 

build_telemetry_string 

0  ; 

extern 

void 

parse_telemetry_string 

0  ; 

extern 

void 

read_tel  erne  try_st  ring 

0; 

extern 

void 

open__tactical_socket 

0  ; 

extern 

void 

shutdown_tactical_socket 

0  ; 

extern 

void 

send_buffer_to__tactical_socket 

0  ; 

extern 

void 

get_string_from_tactical_socket 

0  ; 

extern 

void 

r ec  ord_da  t  a_on 

0 ; 

extern 

void 

r ec  0  rd_da  t  a_o  f  f 

0  ; 

extern 

void 

cage_dg 

0  ; 

extern 

void 

uncage_dg 

0; 

extern 

int 

detect_death_spiral 

0; 

/*  from  parse_ 

functions ,c 

extern 

void 

parse_command_line_flags 

{)  / 

extern 

int 

parse_mission_script_commands 

0  ; 

extern 

void 

•  parse_mission_string__commands 

0  ; 

extern 

void 

p  r i n  t_va 1 i d_k  ey wo  r d s 

{)  ; 

extern 

void 

get_control_constants 

0  ; 

extern 

double 

dsign 

0  ; 

extern 

double 

dtanh 

0; 

/*  File  Scope  Globals  for  use  by  the  Thruster  Speed  Controller  Routines  */ 

double  Inters  =  0.0, 

Int_ls  =  0.0; 

/*  DAC  Values  being  sent  50  props  and  thrusters  */ 


int  v_dls  =  512, 
v_drs  =  512, 
v_dblt  =  512, 
v_dslt  =  512, 
v_dbvt  =  512, 
v__dsvt  =  512; 

double  sin__psi, 
cos_psi, 
cos_phi; 

/*  Dive  Tracker  Variables  */ 
#ifdef  os9 
DT2CLMein  *dt_dmod; 

#endif 


/*  File  Scope  Globals  for  Target  Control  Routines  */ 


int  new_target_update  =  0; 

int  stl000_bytes_expected  =  0; 

double  psi_coinmand_tgt  =  0.0; 

double  time_last_target_update  =  0.0; 

int  SONARPINGED  = •FALSE; 

int  reset_sonar_f liter  =  TRUE; 


double  ST1000_range_kal ; 
double  ST1000_range_kal_dot ; 
double  ST1000_range_kal_ddot; 
doubl e  ST7  2  5_range_ka 1 ; 
double  ST72  5_range_kal__dot; 
double  ST725__range__kal_ddot ; 


main  (argc,  argv) 

int  argc;  char  **argv; 

{ 


/*  Note  K+R  C  function  prototyping  is  due  to  OS-9.  */ 

/*  command  line  arguments.  */ 


if  (TRACE  &&  DISPLAYSCREEN)  printf  ("[start  main:  execution] \n" ) ; 
strcpy  (virtual_world_remote_host_name,  VIRTUAL_WORLD_REMOTE_HOST_NAME)  ; 
strcpy  (tactical_remote_host_naine,  TACTICAL_REMOTE_HOST_NAME)  ; 
dt  =  TIMESTEP; 

parse_command_line_flags  (argc,  argv) ; 
open_device__paths  ()  ; 
kal_init_z  =  TRUE; 

record_data_on  ();  /*  Open  files  for  data  logging  */ 

if  (LOCATIONLAB) 

{ 

open_virtual_world_socket  ();  /*  open  connection  to  virtual  world  */ 

if  (TRACE  ScSc  DISPLAYSCREEN) 

printf  ("\n [LOCATIONLAB  ==  TRUE,  open_virtual_world_socket  ()]\n"); 
if  (strlen  (buffer)  >  0) 

send__buf  fer_to_virtual_world_socket  ()  ; 

/*  SILENT?  send  to  sound  driver  */ 

strcpy  (buffer,  "  A  U  V  virtual  world  socket  is  open"); 
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} 


send_buf  f er_to_virtual_world_socket  ( )  ; 

/*  buffer  containing  message  sent  */ 


if  (TACTICAL) 

{ 

open_tactical_soc}cet  ();  /*  open  connection  to  tactical  level  */ 

strcpy  (buffer,  "  A  U  V  tactical  socket  is  open" ) ; 
if  (DISPLAYSCREEN)  printf  ("[%s\n]",  buffer); 
send_buffer_to_virtual_world_socket  ();/*buffer  containing  message  sent*/ 

if  (LOCATIONLAB  ==  FALSE) 

{ 

#ifdef  os9 

/*  Dive  Tracker  Stuff  */ 
if  (DIVETRACKER) 

{ 

createdmod ( ) ; 

/*  Fork  Dive  Tracker  Process  */ 
if  ( (dt_pid  = 

os9fork ("/rO/div_trac", 0, dt_fork_parmptr, 0,0,0, 0) )  >0) 

{ 

if  (DISPLAYSCREEN) 

printf (" [Dive  Tracker  Process  %d  forked] \n" , dt_pid) ; 

} 

else 

{ 

if  (DISPLAYSCREEN)  printf (" [Can' t  Fork  Dive  Tracker  Process] Vn" ) ; 

} 

} 

#endif 

/*  sleep  gives  5  minutes  to  unhook  wires  and  put  AUV  in  the  water  */ 
if  (DISPLAYSCREEN  ==  FALSE) 

{ 

printf (" [Starting  a  60  second  sleep,  disconnect  Ethernet  cable! ]\n"); 
sleep  (60) ;  /*  disconnect  cable  during  this  interval  */ 

} 

init_pia  ( ) ; 
init_timla  (1); 
uncage_dg  { ) ; 

zero^surfaces  ( )  ; 
command_propellors_of f  ()  ; 
command_thrusters_of f  ()  ; 
thruster_power (1)  ; 
screw_power (1)  ; 
initialize__adcs  ()  ; 
zero_gyro_data  (); 

} 

if  (SONARINSTALLED) 

{ 

if  (LOCATIONLAB  ==  FALSE) 

{ 

initialize_sonar  (); 

set_step_size  (1);  /*  set  step  size  to  0.9  */ 

} 

center__sonar  ();  /*  must  have  open_device_paths  1st  */ 


get_control_constants  ();  /*  announce  filename  as  diagnostic  */ 

strcpy  (buffer,  "EXECUTIONJNITIALIZED")  ; 
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send__buf fer_to_tactical_socket  ();  /*  message  */ 
tsleep (10)  ; 

z  =  read_depth  ();  /*  Read  depth  */ 

2__command  =  z  ; 

if  (LOCATIONLAB) /*pass  initial  position  to  initialize  hydrodynamics  model*/ 
{ 

sprintf  (buffer,  "initial  position  %4.11f  %4.11f  %4.11f  ",  x,  y,  z) ; 
if  (DISPLAYSCREEN)  printf  ("[%s]\n",  buffer); 

send_buf fer_to_virtual_world_socket  ();/*buffer  containing  message  sent*/ 

sprintf  (buffer,  "position  %4.11f  %4,llf  %4.11f  ",  x,  y,  z) ;  /*  silent  */ 
send_buf f er_to_virtual_world_socket  ();/*buffGr  containing  message  sent*/ 


EMAIL_ENTERED  =  FALSE; 

/ 

******************************* ********************************************^ 
do  /*  while  (LOCATIONLAB  &&  LOOPFOREVER)  */ 


(  /*  indefinite  repeat  loop  for  long-duration  lab  testing*/ 

/* - - - */ 


if  (DISPLAYSCREEN) 

{ 

if  (LOCATIONLAB  &&  (EMAIL)  &&  (EMAIL_ENTERED  ==  FALSE)) 

{ 

strcpy  (buffer,  "  Please  Enter  Your  E-mailAddress"); 
send_buf f er_to_virtual__world_socket  ();  /*buffer  containing  msg  sent  */ 
if  (DISPLAYSCREEN)  printf  ("%s  ***  HERE  ***:  ",  buffer); 

strcpy  (email_address,  ""); 
gets  (email_address)  ; 

EMAIL_ENTERED  =  TRUE; 

sprintf  (buffer,  "Thanks"); 

if  (DISPLAYSCREEN)  printf  ("%s  ",  buffer); 

send_buf f er_to_virtual_world_socket  ();  /*buffer  containing  msg  sent  */ 

if  ((int)  (strlen  (email_address)  >2)) 

{ 

sprintf  (buffer,  "%s\n",  email_address) ; 

if  (DISPLAYSCREEN)  printf  ("%s\n",  buffer); 

send_buf fer_to__virtual_world_socket  ();  /*  buffer  sent  */ 

if  ((strcmp  (emaiLaddress,  "brutzman")  1=  0)  && 

(strcmp  (email_address,  "BRUTZMAN")  !=  0)  && 

(strcmp  {email_address,  "brutzman@nps.navy.mil")  0)) 

{ 

emailaddressfile  =  f open (EMAILADDRESS FILENAME, "a" ) ;  /*append  */ 
fprintf  (emailaddressfile,  "%s\n",  email_address) ; 
fclose  (emailaddressfile); 

} 

} 

} 

else  if  (LOCATIONLAB  ==  FALSE)  /*  in  water  */ 

{ 

test_alive  (10,  start_dwell ) ; 

} 

if  (DISPLAYSCREEN)  printf (" \n\nOK ! 1  Starting  the  mission .\n"); 


if  (NOSCRIPT  ==  FALSE)  /*  scriptfile  available  */ 

parse_mission_script_commands  ();  /*  read  initial  script  orders  */ 

/*  ignore  failure  */ 


");  /*  pause  */ 


} 

strcpy  (buffer, 


/  /  /  / 
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send_buf fer_to_virtual_world_soc]cet  ();  /*  buffer  containing  msg  sent  */ 

strcpy  (buffer,  "  A  U  V  is  starting"); 

send_buf fer_to_virtual_world_socket  ();  /*  buffer  containing  msg  sent  */ 

/*  Initialization  of  closed  loop  parameters  */ 


buffer_index  =  0; 

tel erne try_records_saved  =  0; 

mission_leg_counter  =  0; 

end_test  =  FALSE; 

wrap_count  =  0; 

t  =0.0; 

dt_time  =  0.0; 


/* - -  j 

/*  Main  program  operational  loop  code  */ 


if  (TRACE  &&  DISPLAYSCREEN) 

printf  ("[Starting  main  program  operational  loop  code...  ]"); 
nextloopclock  =  clock  ()  +  (int) (dt  *  (double)  CLOCKS_PER_SEC) ; 


/* - 

while  (end_test  ==  FALSE)  /*this  is  the  realtime  main  operational  loop  */ 

/*  when  end_test  ==  TRUE  then  loop  is  done  */ 

closed_loop_control_module  ();  /*  closed  loop  code  is  here  < - */ 

}  /*  end  of  real-time  main  operational  loop  */ 


/*  Kill  Dive  Tracker  Process,  Unlink  Shared  Memory,  and  Cage  Gyro  */ 

if  (LOCATIONLAB  ==  FALSE) 

{ 

cage_dg  ( )  ; 


#ifdef  os9 

/*  Kill  Divetracker  Process  */ 
if  (DIVETRACKER) 

{ 

kill (dt_pid, 0) ; 

/*  Unlink  Shared  Memory  Module  */ 

ul_pid  =  os9exec (os9forkc,argblk[0] ,argblk, environ, 0,0,3) ; 
ul_pid  =  os9exec(os9forkc,argblk[0] ,argblk,environ,0,0,3) ; 
ul_pid  =  os9exec(os9forkc,argblk[0] ,argblk, environ, 0,0,3) ; 

} 

#endif 

} 


/* - 

/*  lab  version  may  repeat  forever  for  long-duration  testing;  */ 

replication_count  ++; 


if 

{ 


} 


(LOCATIONLAB  &&  LOOPFOREVER  &&  DISPLAYSCREEN) 


printf  ("\n[LOOP  FOREVER  enabled,  next  loop  is 
replication_count) ; 

sprintf  (buffer,  "  LOOP  FOREVER  enabled,  next 
send_buffer__to_virtual_world_socket  () ;  /* 

sprintf  (buffer,  "  %d",  replication_count) ; 
send_buffer_to_virtual_world_socket  () ;  /* 


replication  %d.,.]\n", 

loop  is  replication"); 
buffer  msg  sent  */ 

buffer  msg  sent  */ 
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if  (LOCATIONLAB  &&  LOOPFOREVER) 

{ 

/*  reset  amount  of  time  to  wait  for  next  command  */ 
time_next_command  =  0.0; 
t  =0.0; 

if  (DISPLAYSCREEN) 

{ 

printf  ("\nLoop forever  reset  time:  [time_next_coinmand  =  0.0] 
printf  ("  [t  =  0.0]\n"); 

} 

} 


if  (LOOPFILEBACKUP) 

{ 

record_data_of f  ( ) ; 


#ifndef  os9 

if  (DISPLAYSCREEN) 
printf  ("rm 
system  ("rm 
if  (DISPLAYSCREEN) 
printf  ("cp  mission. output 
system  ("cp  mission. output 
if  (DISPLAYSCREEN)  printf  ( 
put , l_second. previous\n"  ); 
system  ("rm 
if  (DISPLAYSCREEN) 
printf  ("cp  mission. output 
system  ("cp  mission. output 

#else 


output . telemetry .previous \n") ; 
output . telemetry .previous"  ); 

.telemetry  output . telemetry .previousXn") ; 
.telemetry  output . telemetry  .previous"  ); 
"rm  out- 


output  .  l__second.  previous" 


); 


.  l_second  output .  l__second.  previous \n"  ); 
.l_second  output . l^second. previous"  ); 


if  (DISPLAYSCREEN) 

printf  ("del  output . telemetry .previousXn" ) ; 

system  ("del  output . telemetry .previous"  ); 

if  (DISPLAYSCREEN) 

printf ("copy  mission. output . telemetry  output . telemetry  .previousXn" ) ; 
system 

("copy  mission. output . telemetry  output . telemetry  .previous"  ); 
if  (DISPLAYSCREEN) 

printf  ("del  output .l_second. previousXn"  ); 

system  ("del  output . l_second. previous"  ); 

if  (DISPLAYSCREEN) 

printf  ("copy  mission.output .  l_second  output . l__second. previousXn"  ); 
system  ("copy  mission. output .l_second  output. 1  second. previous"  ); 

#endif 


if  (LOCATIONLAB) 

{ 

strcpy  (buffer,  "  telemetry  data  backup  complete"); 
send_buffer_to_virtual__world_socket  ();  /*  buffer  msg  sent  */ 

} 

else  /*  don't  bother  backing  up  most  recent  results  */ 

{ 

if  (LOCATIONLAB  &&  LOOPFOREVER) 

{ 

if  (auvtextfile)  rewind  (auvtextfile) ; 
if  (((TACTICAL  ==  FALSE)  |[  ( TACTICAL PARSE ) ) 

ScSc  (auvdatafile  1=  NULL)) 

{ 

rewind  (auvdatafile); 
if  (TRACE  ScSc  DISPLAYSCREEN) 

printf  ("[auvtextfile  &  auvdatafile  rewound  to  "); 
printf  ("output .data .previous  &  output . auv. previous ] Xn" ) ; 

} 

strcpy  (buffer,  "  telemetry  data  backup  skipped"); 

send_buf fer_to_virtual_world_socket  ();  /*  buffer  msg  sent  */ 
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} 


if  (NOSCRIPT  ==  FALSE) 

{ 

if  (auvscriptf ile) 

{ 

f flush  (auvscriptf ile) ; /*  force  completion-f ile  write  */ 
if  (fclose  (auvscriptf ile)  ==  0) 

{ 

if  (DISPLAYSCREEN) 

printf  ("[success  closing  auvscriptf ile  mission. script .backup] \n") ; 

} 

else  if  (DISPLAYSCREEN) 

printf  ("[failure  closing  auvscriptfile  mission. script .backuo] \n" ) ; 

} 

orders 

if  (auvordersf ile)  f flush (auvordersf ile) ; /*force  write  completion*/ 
if  (auvordersf ile)  fclose (auvordersf ile) ; 


/* 

#ifndef  os9 

if  (DISPLAYSCREEN) 

printf  ("rm  mission. output .ordersXn") ; 

system  ("rm  mission. output . orders"  ); 

if  (DISPLAYSCREEN) 

printf  ("mv  mission. output .orders .backup  mission. output .orders\n" ) ; 
system  ("mv  mission. output, orders. backup  mission. output. orders"  ); 

#else 

if  (DISPLAYSCREEN) 

printf  ("del  mission. output .orders\n" ) ; 

system  ("del  mission. output . orders"  ); 

if  (DISPLAYSCREEN) 

printf  ("copy  mission. output . orders .backup  mission. output .orders\n" ) ; 
system  ("copy  mission. output . orders .backup  mission. output .orders"  ); 
if  (DISPLAYSCREEN)  printf  ("del  mission . output . orders .backup\n\n" ) ; 
system  ("del  mission. output . orders .backup"  ); 

#endif 

*/ 

#ifndef  os9 

sprintf  (buffer,  "rm  %s.backup\n",  AUVORDERSFILENAME) ; 

#else 

sprintf  (buffer,  "del  %s .backupXn" ,  AUVORDERSFILENAME); 

#endif 

if  (DISPLAYSCREEN)  printf  ("%s\n",  buffer); 
system  (buffer) ; 

#ifndef  os9 

sprintf  (buffer,  "cp  %s  %s .backupXn" ,  AUVORDERSFILENAME,  AUVORDERSFILENAME); 
#else 

sprintf  (buffer,  "copy  %s  %s. backupXn",  AUVORDERSFILENAME,  AUVORDERSFILE¬ 
NAME)  ; 

#endif 

if  (DISPLAYSCREEN)  printf  ("%sXn",  buffer); 

system  (buffer) ; 

} 

if  (NOSCRIPT)  /*  copy  default  orders  file  for  noscript  mode  */ 

#ifndef  os9 

sprintf  (buffer,  "rm  % s. backupXn", AUVORDERSFILENAME) ; 
if  (DISPLAYSCREEN)  printf  ("%sXn",  buffer); 

system  (buffer) ; 

sprintf  (buffer,  "cp  %s  %s. backupXn",  AUVORDERSFILENAME,  AUVORDERSFILENAME); 
if  (DISPLAYSCREEN)  printf  ("%sXn",  buffer); 

system  (buffer) ; 

sprintf  (buffer,  "cp  %s. noscript  %sXn",  AUVORDERSFILENAME,  AUVORDERSFILENAME); 
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if  (DISPLAYSCREEN)  printf  ("%s\n",  buffer); 

system  (buffer) ; 

sprintf  (buffer,  "chitiod  +w  %s\n",  AUVORDERSFILENAME) / 

if  (DISPLAYSCREEN)  printf  {"%s\n",  buffer); 

system  (buffer) ; 

sprintf  (buffer,  "cp  %s  %s\n",  TELEMETRY? ILENAME,  AUVDATAFILENAME) ; 
if  (DISPLAYSCREEN)  printf  ("%s\n'',  buffer); 

system  (buffer) ; 

sprintf  (buffer,  "cp  %s  %s\n'^  TELEMETRY? ILENAME,  AUVTEXTFILENAME)  ; 
if  (DISPLAYSCREEN)  printf  ("%s\n",  buffer); 

system  (buffer) ; 

#endif 

} 

e-mail  - 

if  (replication^count  <=  2)  /*  only  send  e-mail  once  */ 

tifndef  os9 

sprintf  (buffer,  "rm  %s\n", 

#else 

sprintf  (buffer,  "del  %s\n", 

#endif 

if  (DISPLAYSCREEN)  printf  ("%s\n",  buffer);, 
system  (buffer); 

#ifndef  os9 

sprintf  (buffer,  "cp  %s  %s\n",  AUVINFOFILENAME, 

#else 

sprintf  (buffer,  "copy  %s  %s\n",  AUVINFOFILENAME, 

#endif 

if  (DISPLAYSCREEN)  printf  ("%s\n",  buffer); 
system  (buffer) ; 

#ifndef  os9 

sprintf  (buffer,  "cat  %s  »  %s\n",  AUVSCRIPTFILENAME,  AUVEMAILFILENAME)  ; 

#else 

sprintf  (buffer,  "list  %s  »  %s\n",  AUVSCRIPTFILENAME,  AUVEMAILFILENAME); 
#endif 

if  (DISPLAYSCREEN)  printf  ("%s\n",  buffer); 
system  (buffer) ; 

if  ((int)  (strlen  (email_address)  >=  3)  &&  (EMAIL)) 

sprintf  (buffer,  "mail  %s  <  %s",  email_address,  AUVEMAILFILENAME); 
#ifndef  os9 

if  (DISPLAYSCREEN)  printf  ("%s\n",  buffer); 
system  (buffer) ; 

#else 

/*  system  (buffer);  /*  e-mail  not  available  directly  on  OS-9  */ 

send_buf fer_to_virtual_world_socket  ();  /*  buffer  msg  sent  anyway  */ 

#endif 

} 

}  /*  end  if  (replication_count  <=  2)  */ 

/*  permit  changing  the  vehicle  mission  during  continuous  lab  testing  */ 
if  (LOCATIONLAB  &&  LOOPFOREVER) 

{ 

get_control_constants  () ; 

nextloopclock  =  clock  ()  +  (int) (dt  *  (double)  CLOCKS_PER_SEC); 
record_data_on  ( ) ; 

strcpy  (buffer,  "  Load  mission  again"); 
send_buffer_to_virtual_world__socket  {)  ; 

/*  buffer  containing  message  sent  */ 


AUVEMAILFILENAME) ; 
AUVEMAILFILENAME) ; 

AUVEMAILFILENAME) ; 
AUVEMAILFILENAME) ; 
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}  while  (LOCATIONLAB  &&  LOOP  FOREVER)  ; /*  end  of  lab  infinite  loop  {if  any)  */ 
/************************************************************************★/ 

command__propellors_off  ();  /*  all  done,  turn  them  off  */ 

if  (TRACE  ScSc  DISPLAYSCREEN) 

printf  ("[all  done,  send  'kill'  message  to  virtual  world  dynamics ] \n" ) ; 

strcpy  (buffer,  "kill");  /*  must  start  with  'shutdown'  to  die  */ 

send_buffer_to_virtual__world_socket  ();  /*buffer  containing  message  sent  */ 

shutdown_virtual_world_socket  {);  /*  close  connection  to  virtual  world  */ 

close_device_paths  (); 

record_data_of f  () ; 

if  (TRACE  ScSc  DISPLAYSCREEN) 

printf  (" [finishing  main:  fflush  (stdout) ,  fflush  (stderr) ] \n") ; 

fflush  (stdout);  /*  force  completion  of  screen  write  */ 
fflush  (stderr);  /*  force  completion  of  error  write  */ 

if  (TRACE  &&  DISPLAYSCREEN)  printf  {"[main  exit:  return  (0)]\n"); 

#ifndef  os9 

if  (DISPLAYSCREEN)  printf  ("gnuplot  auv_plot_l_second. gnu\n" ) ; 
system  ("gnuplot  auv_plot_l__second 'gnu" ) ;  /*  display  plotted  results  */ 
#endif 

return  (0);  /*  main  program  exit  */ 

}  /*  end  main  program  block,  execution  is  complete  */ 

/*********★****************************************************** ***********^ 
/************************************★****★*********************************/ 

void  closed_loop_control_module  ()  /*  executed  each  time  step  */ 

double  volts_per_dac  =  0.046875; 

double  lateralMult;  /*  multiple  for  lateral  thruster  voltage  */ 
int  dt__rangel,  dt_range2; 

if  (TRACE  &&  DISPLAYSCREEN) 

printf  {"[start  closed_loop_control_module] \n" ) ; 

if  ((LOCATIONLAB  ==  FALSE)  &&  (BENCHTEST==FALSE)  &&  (HALTSCRIPT  ==  FALSE)) 

if  ( (computer_voltage  =  read_computer_batt€ry_voltage ( ) )  <  20.0) 

{ 

HALTSCRIPT  =  TRUE; 
if  (DISPLAYSCREEN) 

^printf ("Low  Computer  Voltage  Detected:  %3 . If \n" , computer_voltage) ; 

if  ( (motor^voltage  =  read_motor_gyro_battery_voltage ( ) )  <  20.0) 

HALTSCRIPT  =  TRUE; 
if  (DISPLAYSCREEN) 

printf ("Low  Motor  Voltage  Detected:  %3 . If \n" ,motor_voltage) ; 

if  (leak^check  ()) 

{ 

HALTSCRIPT  =  TRUE; 

if  (DISPLAYSCREEN)  printf ("Leak  DetectedXn" ) ; 

) 

if  (2_kal  >  6.0) 
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{ 

HALTSCRIPT  =  TRUE; 

if  (DISPLAYSCREEN)  printf ( "Depth  Exceeded\n" ) ; 

} 

if  (DIVETRACKER  &&  (dt^time  +  30.0  <=  t) ) 

{ 

HALTSCRIPT  =  TRUE; 
if  (DISPLAYSCREEN) 

printf ("Loss  of  Dive  Tracker  for  30  SecondsXn"); 

} 

if  ( (SONARINSTALLED)  &&  (SONARSCANMODE  ==!)&& 

(ST1000_range_kal  >  0.0)  &&  (ST1000_range_kal  <  3.0)) 

{ 

HALTSCRIPT  =  TRUE; 
if  (DISPLAYSCREEN) 

printf  ("Collision  Avoidance  Invoked:  ST1000_range_kal  =  %6.31f\n", 
ST1000_range_kal ) ; 

} 

} 

if  (HALTSCRIPT) 

{ 

execute_shutdown_script  ( )  ; 

} 

/*  Read  Sensors  and  Conimunicate  with  Virtual  World  ********************/ 

if  (SONARPINGED  ==  TRUE) 

{ 

AUV_ST1000_range  =  read_sonar  (); 

if  {(fabs  (AUV_ST1000_range  -  ST1000_range_kal)  >=  3.0)  il 
{AUV_ST1000_range  <=  0.001)  II 
(ST1000_range_kal  <=  0.001)) 
reset_sonar_f liter  =  TRUE; 
kalman_sonarl000  (AUV_ST1000_range) ; 

SONARPINGED  =  FALSE; 

fprintf  (stl000datafile,"%6.11f  %6.31f  %6.31f  %6.31f  %6.31f  %6.31f\n", 
t,  AUV_ST1000_range,  ST1000_range_kal,  AUV_ST1000_bearing, 
ST1000_range_kal  *  cos  (radians  (AUV_ST1000_bearing)  ) , 
ST1000_range_kal  *  sin  (radians  (AUV_ST1000_bearing)  ) )  ; 


control_sonar  (); 

speed  =  read_speed  ();  /*  Added  by  D.  Marco  1-12-96  */ 

rpm  =  (port_rpm_command  +  stbd_rpin_coininand)  /  2.0; 
clamp  (&  rpm,  700.0,  -700.0,  "rpm");  /*  bound  maximum  RPM  */ 

if  (TRACE  &&  DISPLAYSCREEN) 

printf  ("[clamp  (&  rpm,  700.0,  -700.0,  \"rpm\")  complete) \n" } ; 

/*  Main_Motor  RPM  Control  *********************************************/ 

/*  note  thruster  use  does  not  preclude  propeller  use  */ 

if  (LOCATIONLAB)  /*  rpm  model  assumes  instantaneous  response  */ 

{ 

port_rpm  =  port_rpm_command; 
stbd_rpm  =  stbd_rpm__command; 

} 

else  /*  in  water  =>  propeller  rpms  are  controlled  so  read  actual  value  */ 
{ 

port_rpra  =  read_portjmotor_rpm  ( )  ; 
stbd_rpm  =  read_stbd_motor_rpm  ()  ; 


162 


/*  if  using  virtual  world  dynamics,  network  is  source  of  values  ««««  */ 

phi  =  read_roll_angle  ();  /*  read  roll  angle  */ 

cos_phi  =  cos  (radians  (phi)); 

theta  =  read_pitch_angle  {) ;  /*  read  pitch  angle  */ 

psi  =  readjsi  ();  /*  Read  psi/heading  */ 

sin_psi  =  sin  (radians  (psi)); 
cos_psi  =  cos  (radians  (psi)); 

p  =  read_roll_rate_gyro  ();  /*  read  roll  rate  */ 

q  =  read__pitch_rate_gyro  ();  /*  read  pitch  rate  */ 

r  =  normalize2  (psi  -  psi_iinl) /dt;  /*  differentiate  to  get  r  */ 
psi__iml  =  psi; 


/* 

r 

=  read  yaw  rate  gyro 

{); 

/*  Read  yaw  rate 

z 

=  read_depth 

0; 

/*  Read  depth 

*/ 

kalman_z  (z); 

if 

(TRACE  &&  DISPLAYSCREEN) 
printf  ("[z=%5.2f,  z_kal 

=%5.2f] 

l\n",  z,  z_kal); 

if  (fabs  {z_kal)  <  0,0001)  z_kal  =  0,0; 

if  (fabs  (z_dot_kal)  <  0,0001)  z_dot_kal  =  0,0; 

if  (fabs  (z_ddot_kal)  <  0.0001)  z_ddot_kal  =  0.0; 

z_dot  =  z_dot_kal; 

w  =  z_dot_kal;  /*  look  out!!  ««  */ 

/*  note:  in  laboratory  using  virtual  world,  values  above  are  superceded  */ 

/*  estimate  X  and  Y  with  Mathematical  Model  or  Dead  Reckoning  */ 

if  (LOCATIONLAB  ==  FALSE)  /*  in-water,  perform  a  valid  dead  reckon  */ 

XY_model_est  (  (v_dls  -  512)  *  volts_per_dac, 

(v_drs  -  512)  *  volts_per_dac, 

AUV_bow_lateral,  AUV__stern_lateral , 
AUV_:.oceancurrent_x,  AUV_oceancurrent_y ,  TRUE  }  ; 

else  /*  virtual  world  providing  sensor  inputs  */ 

X  +=  (speed  *  dt  *  cos_psi); 

if  (fabs{x)  <=  0.0001)  x  =  0.0;  /*  prevent  OS-9  gasping  */ 

y  +=  (speed  *  dt  *  sin_psi); 

if  (fabs(y)  <=  0,0001)  y  =  0.0;  /*  prevent  OS-9  gasping  */ 

X  +=  AUV_oceancurrent_x  *  dt; 
y  +=  AUV_oceancurrent_y  *  dt; 


if  (TRACE  ScSc  DISPLAYSCREEN) 

{ 

printf  ("  [AUV_oceancurrent_x  =  %3.1f,",  AUV_oceancurrent_x)  ; 
printf  ("  AUV^oceancurrent^y  =  %3,lf,",  AUV_oceancurrent_y) ; 
printf  {"  AUV_oceancurrent_z  =  %3.1f]\n'',  AUV  oceancurrent  z)  ; 

} 

/*  Control  laws  ****  NOTE:  all  k_  constants  must  be  (+)  positive  ****  */ 
#ifdef  os9 

/*  Update  Dive  Tracker  Ranges  */ 

if  (DIVETRACKER  &&  (CLReaddmod  (Scdt_rangel ,  &dt_range2 )  ==NEW_DATA)  && 
(dt_rangel  <  10000)  &&  (dt_rangG2  <  10000)  && 

(dt_rangel  >  0)  &&  (dt_range2  >0)) 

{ 

divetracker_rangel  =  (double)  dt_rangel  /  12.0; 
divetracker_range2  =  (double)  dt_range2  /  12.0; 
dt_time  =  t; 
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if  ((TRACE)  &&  (DISPLAYSCREEN) ) 

printf ("Divetracker  Ranges:  %f  %f  %f\n", 

t , divetracker_rangel , divetracker_range2 ) ; 

} 

else 

{ 

if  (z_kal  <=  2.0)  dt_tiine  =  t; 

} 

#endif 

waypoint_distance  =  sqrt  (  (x  -  x_command)  *  (x  -  x_command) 

+  (y  “  y_command)  *  (y  ~  y_coimnand) )  ; 

/*  calculate  depth  error  OK  prior  to  death  spiral  check  */ 

depth_error  =  (z_command  -  2_kal); 

/*  constrain  depth_error  to  +-  15.0  feet  to  prevent  going  vertical  */ 

/*  and  enable  stable  pitch  angle  even  on  large  depth  changes  */ 

clamp  (&  depth_error,  -15.0,  15.0,  "depth_error" ) ;  /*  feet  */ 

/*  Zero  thruster  commands  */ 


AUV_bow_vertical  =  0.0; 
AUV_stern_vertical  =  0.0; 
AUV__bow_lateral  =  0.0; 
AUV_stern_lateral  =  0.0; 
delta_rudder  =  0.0; 
delta_planes  =  0.0; 


/*  Recompute  new  thrustercommands  depending  on  control  mode  ************/ 

if  (HOVERCONTROL)  compute_hover_controls  (); 

else  if  (TARGETCONTROL)  compute_target_controls  (); 

else  if  (WAYPOINTCONTROL)  compute_waypoint_controls  {); 

else  if  (LATERALCONTROL)  compute_lateral_controls  (); 

else  if  (ROTATECONTROL)  compute_rotate_controls  (); 

else  if  (RECOVERYCONTROL)  compute_recovery_controls  ()  ; 

else  if  (THRUSTERCONTROL)  compute_lateral_thrusters  () ; 


if  (TRACE  &&  DISPLAYSCREEN) 

{ 

printf  ("Pre-sqrt  thruster  control  calculated  values:\n"); 
printf.  ("AUV_bow_vertical  =  %6.3f\n",  AUV_bow_vertical ) ; 
printf  ("AUV_stern_vertical  =  %6.3f\n",  AUV_stern_vertical ) ; 
printf  ("AUV_bow_lateral  =  %6.3f\n",  AUV_bow_lateral) ; 
printf  ("AUV_stern_lateral  =  %6.3f\n",  AUV_stern_la teral ) ; 


/*  convert  to  signed  sqrt  to*  account  for  vol ts-to-thrust  relationship  */ 

/*  different  multiple  required  between  lab  and  auv  because  of  polarity  */ 
/*  discrepancy  between  virtual  world  and  actual  auv  */ 

if  (LOCATIONLAB)  lateralMult  =  2.0; 
else  lateralMult  =  -2.0; 

/*  2.0  *  sqrt{6.0)=  4.8989  SQR  6  =  2.449  */ 

AUV_bow_vertical  =  4.8989  *  dsign  (AUV_bow_vertical  ) 

*  sqrt  (fabs  (AUV_bow_yertical  )); 

AUV__stern_vertical  =  4.8989  *  dsign  (AUV_stern_vertical ) 

*  sqrt  (fabs  (AUV_stern_vertical) ) ; 
AUV_bow_lateral  =  lateralMult  *  2.449  *  dsign  (AUV_bow_lateral  ) 

*  sqrt  (fabs  (AUV_bow_lateral  )); 
AUV_stern_lateral  =  lateralMult  *  2.449  *  dsign  (AUV_stern_lateral  ) 

*  sqrt  (fabs  (AUV_stern_lateral  )); 

if  (TRACE  &&  DISPLAYSCREEN) 

{ 
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printf  ("Post-sqrt  thruster  control  calculated  values :\n"); 
printf  ("AUV_bow_vertical  =  %6.3f\n",  AUV_bow__vertical ) ; 
printf  { "AUV_stern_vertical  =  %6.3f\n",  AUV_stern_vertical ) ; 
printf  ( "AUV_bow_lateral  =  %6.3f\n",  AUV_bow_lateral ) ; 
printf  ( "AUV_stern_lateral  =  %6.3f\n",  AUV_stern_laterai ) ; 

} 

/*  fins  reset  except  in  TARGETCONTROL  and  RECOVERYCONTROL  modes  */ 

if  {  (TARGETCONTROL  ==:  FALSE)  &&  (RECOVERYCONTROL  ==  FALSE)  ) 
compute_f in_controls  (); 

/*  constrain  thruster  orders  +/-  24.0  volts  ==  3820  rpm  no-load  */ 

/*  constrain  propeller  orders  +/-  700  rpm  no-load  */ 

clamp  (&  AUV_bow_vertical,  -24.0,  24.0,  "AUV_bow_vertical") ; 

clamp  (&  AUV__stern_vertical,  -24.0,  24.0,  "AUV_stern_vertical" ) ; 
clamp  (&  AUV_bow__lateral,  -24.0,  24.0,  "AUV_bow_lateral" ) ; 

clamp  (Sc  AUV_stern_lateral,  -24.0,  24.0,  "AUV_stern_lateral" )  ; 

clamp  (Sc  port_rpm_coramand,  -700.0,  700.0,  "port_rpm_command" )  ; 

clamp  (Sc  stbd_rpm__command,  -700.0,  700.0,  "stbd__rpm_command" )  ; 

/*  Record  Control  Orders  to  Orders  File  */ 

if  (  (NOSCRIPT  ==  FALSE)  ScSb 

( (HOVERCONTROL)  il  (TARGETCONTROL)  || 

(WAYPOINTCONTROL)  (  I  (ROTATECONTkOL)  I  1 
( THRUSTERCONTROL )  )  ) 
f printf  (auvordersf ile, 

"%6.1f  %6.1f  %5.1f  %5.1f  %5.1f  %6.1f  %6.1f  %6.1f  %6.1f  %5.1f  %5.1f  %5.1f 

%5.1f\n", 

t,  psi_command,  x_command,  y_command,  z^command, 
p  o  r  t  _r pm_c  omma  nd ,  s  t  bd_r pm_c  omma  nd , 
rudder_command,  planes_command, 

AUV_bow__vertical ,  AUV_stern_vertical , 

AUV_bow_lateral ,  AUV_stern_lateral )  ; 

/*  command  thruster  and  propel lor  orders  */ 

command_motor  (AUV_bow_vertical ,  BOW^VERTICAL) ; 

command_motor  (AUV_stern_vertical,  STERN__VERTICAL) ; 
command_motor  (AUV_bow_lateral ,  BOW_LATERAL) ; 

command_motor  (AUV_stern_lateral ,  STERN_LATERAL) ; 
command_motor  (port_rpm_command,  PORT_PROP)  ; 

command_motor  (stbd_rpm_command,  STBD_PROP)  ; 

/*  Send  commands  to  rudders  and  planes  ********************************/ 

command_rudder  (delta_rudder) ; 
command^planes  (delta_planes) ; 


/*  send  telemetry  to  tactical  level  and  data  recording  files  -  ~  ~ 

record_data  ( ) ;  • 

/*  read  commands  from  tactical  level  - 

/*  if  (TACTICAL)  read_parallel_port  ();[old  code]  now  uses  socket*/ 
/*  update  simulation  clock  "t"  - 


t  =  t  +  dt; 

fflush  (stdout);  /*  force  completion  of  screen  write  */ 
currentloopclock  =  clock  ( ) ; 

if  (TRACE  ScSc  REALTIME  ScSc  DISPLAYSCREEN) 
printf (" [Unused  Loop  Time:  %5.4f\n", 

(float) (nextloopclock  -  currentloopclock)  /  (float)  CLOCKS_PER_SEC) ; 

if  ((REALTIME)  ScSc 


165 


{ 


(currentloopclock  <  nextloopclock) ) 


if  (TRACE  &&  DISPLAYSCREEN) 

{ 

printf  ("currentloopclock  =  %ld,  nextloopclock  =  %ld\n", 
currentloopclock,  nextloopclock) ; 

printf ("timestep  dt  =  %5.3f  seconds  (corresponding  clock  ticks  =  %d)\n", 
dt,  (int) (dt  *  (double)  CLOCK S_PER_S EC) ) ; 
printf  ("Busy  wait  until  system  clock  reaches  simulation  clock,  "); 
printf  ("loop  duration  =  %5.3f\n", 

( (double)  currentloopclock  -  (double)  nextloopclock) 

/  CLOCKS_PER_SEC) ; 

} 

while  (currentloopclock  <  nextloopclock) 

{ 

currentloopclock  =  clock  ();  /*  %%%%%  busy  wait  %%%%%  */ 

} 

if  (TRACE  &&  DISPLAYSCREEN) 

{ 

printf  ("Busy  wait  complete,  loop+wait  duration  =  %5.3f,  ", 
((double)  currentloopclock  -  (double)  nextloopclock) 

/  CLOCKS_PER_SEC) ; 

printf  ("current  clock  ()  =  %ld\n",  currentloopclock); 

} 

} 

else  if  ((REALTIME  ==  FALSE)  &&  LOCATIONLAB  &&  DISPLAYSCREEN  &&  TRACE) 

{ 

printf  ("No  busy  wait,  loop  duration  =  %5.3f,  ", 

(  (double)  currentloopclock  -  (double)  nextloopclock) 

/  CLOCKS_PER_SEC)  ; 

printf  ("current  clock  ()  =  %ld\n",  currentloopclock); 

} 


nextloopclock  =  clock  ()  +  (int) (dt  *  (double)  CLOCKS_PER_SEC) ; 

/*  determine  if  closed_loop_control_module  ()  is  done;  if  so, 

/*repeat  call  to  closed__loop_control_module  ()  for  each  time  step  -  -  */ 

/*this  needs  to  be  3-way  logic  file  I  tactical  i  keyboard  ««««««<  */ 


if  ( (KEYBOARDINPUT)  il  (TACTICAL)) 

{ 

parse_mission__script_commands  ();  /*  get  next  script  orders  read  */ 

/*  ignore  failure  */ 

/*  we  are  reading  from  the  mission  script  file  in  these  cases  */ 

) 

else  if  (auvdatafile  ==  NULL)  /*  file  never  opened,  loop  and  open  it  */ 
{ 

end_test  =  TRUE; 


} 

else  if  (NOSCRIPT)  l'^  scriptfile  not  yet  closed,  read  more  */ 

{ 

/*  ignore  script,  do  not  parse_mission_script_commands  ();  */ 

} 

else  if  (feof  (auvscriptfile)  &&  (t  >  time_next_command) )  /*  all  done  */ 

{ 

if  (TRACE  &&  DISPLAYSCREEN) 

printf  ("end_test  NOSCRIPT  ==  TRUE,  set  TRUE\n"); 
end_test  =  TRUE; 

} 

else  if  (t  >  time_next_command)  /*  scriptfile  not  yet  closed,  read  more  */ 

{ 

if  (TRACE  &&  DISPLAYSCREEN) 

printf  ("\n[read  more  from  parse_mission_script_commands] \n") ; 
parse_mission_script_commands  ();  /*  get  next  script  orders  read  */ 

/*  ignore  failure  */ 

} 

/*  else  not  done  executing  current  script  command,  continue/don't  block  */ 
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if  (TRACE  &&  DISPLAYSCREEN) 

printf  ("\n(time_next_coininand  =  %5.1f]\n",  time_next_cominand)  ; 


if  (TRACE  ScSc  DISPLAYSCREEN) 

printf  ("\n[finish  closed_loop_control_inodule  ()]\n"); 


if 

{ 


} 


(end__test ) 


command^mo to r 
coinmand_ino  to  r 
command^mo t o r 
c  ommand^mo  tor 
command^motor 
command_motor 


(0.0,  BOW_VERTICAL); 
(0.0,  STERN^VERTICAL ) ; 
(0.0,  BOW_LATERAL); 
(0.0,  STERN_LATERAL ) ; 
(0.0,  PORT_PROP) ; 

(0.0,  STBD_PROP); 


return; 

}  /*  end  closed_loop_control_module  ()  */ 

/***************************************************************************/ 

/*  Control  Functions  For  Various  Control  Modes  */ 

/************★*************************************************************★/ 

void  coinpute_hover_controls  () 

{ 

if  (TRACE  ScSc  DISPLAYSCREEN) 

printf  ("[begin  compute_hover_controls ] \n" )  ; 

coinpute_vertical_thrusters  (); 

/*  Distant  hoverpoint  uses  waypoint  control  until  closer  */ 
if  ( (waypoint_distance  >  standoff_di stance  +  20.0)  && 
(detect_death_spiral  {)  ==  FALSE)) 

{ 

if  (TRACE  ScSc  DISPLAYSCREEN) 

printf (" [Hoverpoint  switch  to  WAYPOINTCONTROL] \n" ) ; 

port_rpin_conimand  =  700; 
stbd_rpin_coinmand  =  700; 

WAYPOINTCONTROL  =  TRUE; 

DEADSTICKRUDDER  =  FALSE; 

DEADSTICKPLANES  =  FALSE; 
coinpute__waypoint_controls  (); 

} 

/*  close  hoverpoint  uses  hover  control  */ 
else 
{ 

WAYPOINTCONTROL  =  FALSE; 
psi_command  =  psi_coniinand_hover; 

/*  report  STABLE  to  tactical  level  once  hoverpoint  reached  */ 

if  (  (HOVERCONTROL)  ScSc  (REPORTSTABLE)  ScSc 

( (waypoint_distance  <  standof f_distance)  && 

(fabs  (  depth_error)  <  standof f_distance)  && 

(fabs  (normalize2  (psi  -  psi_cominand)  )  <2.5  /*  degrees  */  ))) 

{ 

if  ((TACTICAL)  ScSc  (GPSFIXINPROGRESS  ==  FALSE)) 

{ 

REPORTSTABLE  =  FALSE; 
strcpy  (buffer,  "STABLE  HOVER" ) ; 
if  (DISPLAYSCREEN)  printf  ("\n[%s]\n",  buffer); 
send_buf fer_to_tactical_socket  ();  /*  message  */ 

} 
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} 


/*  Compute  Control  Settings  */ 
waypoint_angle  =  normalize  (degrees  (atan2z 

(y^command  -  y,  x_command  -  x) )  ) 
track_angle  =  normalize  (waypoint_angle  -  psi); 
along_track_distance=  cos  (radians  (track_angle) )  * 

way  point__di  stance; 

cross_track_distance=  -sin  (radians  (track_angle) )  * 

waypo i n  t_di s  tance; 

port_rpin_command  =  k_propeller_hover  *  along_track_distance 

-  k_propeller_current  *  AUV_oceancurrent_x 

*  cosjsi 

-  k__p r ope ller_cur rent  *  AUV__oceancurrent_y 

*  sin_psi 

-  k_surge_hover  *  u; 

stbd_rpm_command  =  port_rpm_command; 

if  (TRACE  ScSc  DISPLAYSCREEN) 

{ 

printf  { " \nHOVERCONTROL : \n" ) ; 

printf  ( "psi_command  =  %5.1f,  ",  psi_command)  ; 

printf  ("X  =  %5.1f,  y  =  %5.1f\n",  x,  y) ; 

printf  ("waypoint_di stance  =  %5.1f,  track_angle  =  %5.1f\n", 
waypoint_distance,  track_angle) ; 

printf  ("along_track_distance  =  %5.1f,  ",  along_track_distance)  ; 
printf  ("cross_track_dis tance  =  %5,lf\n",  cross_track_distance) ; 
printf  ("port_rpm  &  stbd_rpm  =  %5.1f\n",  port_rpm) ; 


AUV_bow_lateral  =  -  (  -  k_thruster_psi  *  normalize2  (psi-psi_command) 

-  k_thruster_r  *  r) 

+  k__thruster_hover  *  cross_track_di stance 

-  k_thruster_current  *  AUV_oceancurrent_x 

*  sin_psi 

+  k__thruster_current  *  AUV_oceancurrent_y 

*  cos_psi 

+  k_sway_hover  *  v; 

AUV_stern_lateral  =  (  -  k_thruster_psi  *  normalize2  (psi-psi_command) 

-  k_thruster_r  *  r) 

+  k_thruster_hover  *  cross_track_distance 

-  k_thruster_current  *  AUV_oceancurrent_x 

*  sin_psi 

+  k_thruster__current  *  AUV_oceancurrent_y 

*  cosjsi 

+  k_sway_hover  *  v; 


/*  Extend  time  till  next  command  if  not  at  hover  point  yet  */ 
if  ( (HOVERCONTROL)  &&  (GPSFIXINPROGRESS  ==  FALSE)  && 

{ (waypoint_distance  >  standoff_di stance)  I  I 
(fabs  (depth_error)  >  standoff_distance)  i I 

(fabs  (normalize2  (psi  -  psi__command)  )  >  10.0  /*  degrees  */  ))) 

/*  cylinder  test  */ 

{ 

/*  still  not  at  the  hoverpoint  */ 
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if  (TRACE  &Sc  DISPLAYSCREEN)  printf ( " [HOVERCONTROL  cylinder  test]"); 
/*  continue  until  hoverpt  reached  without  further  script  orders  */ 
tiine__next_coTniT\and  =  t  +  2.0  *  dt; 

} 

if  (TRACE  &&  DISPLAYSCREEN) 

printf  ("[end  compute_hover_controls] \n" ) ; 

return;  /*  end  coinpute_hover__controls  ()  */ 

} 

/************************************************************************ ***^ 

void  compute_recovery_controls  () 

{ 

/*  int  TRACE  =  TRUE;  */ 

static  double  last_range_f rom_lef t  =  0.0; 

doub  1  e  r ange_f  r oin__l  eft, 
r  ange_f  r  om_r  i  gh  t , 
s  ide_r  ange__e  r  ro  r , 
s i de_range_ra t e ; 

if  (TRACE  ScSc  DISPLAYSCREEN) 

printf  ("[Begin  compute_recovery_controls]\n"); 

range_f rom_recovery_pt  ~=  u  *  dt; 

/*  report  STABLE  to  tactical  level  once  hoverpoint  reached  */ 

if  ( (REPORTSTABLE)  &&  ( range_f rom_recovery_pt  <=  0.0)} 

{ 

if  (TACTICAL) 

{ 

REPORTSTABLE  =  FALSE; 
strcpy  (buffer,  "STABLE  RECOVERY" ) ; 
if  (DISPLAYSCREEN)  printf  ("\n[%s]\n",  buffer); 
send_buf fer_to_tactical_socket  ();  /*  message  */ 

} 

} 

/*  if  both  sonars  are  not  in  place,  hover  in  place  */ 
if  ((normalize  (AUV_ST1000_bearing)  >286.0)  II 
(normalize  (AUV_ST1000_bearing)  <  285.0)  M 
(normalize  (AIJV_ST725_bearing)  >  75.0)  11 

(normalize  (AUV_ST725_bearing)  <74.0)) 

{ 

if  (TRACE  &&  DISPLAYSCREEN) 

{ 

printf  ("[Using  hover  control  until  sonar  in  place] \n"); 
printf  ("  [AUV_ST1000_bearing  %7 . 31f  ]  \n" ,  AUV_ST1000_bearing)  ; 
printf  (" [AUV_ST725_bearing  %7.31f ] \n" , AUV_ST725„bearing) ; 

compute_hover_controls  ( ) ; 

} 

else 

{ 

kalman_sonar725  (AUV_ST725_range) ; 

range_from_left  =  fabs  (sin  (radians  (AUV__ST1000_bearing) ) ) 

*  ST1000_range_kal ; 

range_from_right  =:  fabs  (sin  (radians  (AUV_ST72  5_bearing)  )  ) 

*  ST725_range_kal ; 

side_range__error  =  (range_f rom_lef t  ~  range_f rom_right )  /  2,0; 

if  (NEWRECOVERYCOMMAND) 

{ 

side_range_rate  =0.0; 

NEWRECOVERYCOMMAND  =  FALSE; 

} 
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else 

{ 

side_range_rate  =  ( range_f rom__lef t  -  last_range_f rom^lef t )  /  dt; 

} 

last__range_f  rom_lef  t  =  range_from_lef  t  ; 
conipute_vertical_thrusters  (); 

/*  Compute  required  propeller  power  */ 

port_rpm_command  =  k_propeller_hover  *  range_f rom_recovery__pt 

-  k_propeller_current  *  AUV_oceancurrent_x 

*  cos_psi 

-  k_propeller_current  *  AUV_oceancurrent_y 

*  sin_psi 

-  k„surge_hover  *  u; 
stbd__rpm_command  =  port_rpm_coiTimand; 

/*  Compute  lateral  thruster  power  */ 

AUV_bow__lateral  =  -  (  -  k_thruster_psi  *  normal! ze2  (psi“psi_command) 

-  k_thruster_r  *  r) 


+  k_thruster_hover  *  side_range_error 


-  k_thruster_current  *  AUV_oceancurrent_x 

*  sin_psi 

+  k_thruster_current  *  AUV_oceancurrent_y 

*  cos_psi 

+  k_sway_hover  *  side_range_rate; 

AUV__stern_lateral  =  (  -  k_thruster_psi  *  normalize2  {psi-psi_command) 

-  k_thruster_r  *  r) 


} 


+  k_thruster_hover  *  side_range_error 

t 

-  k_thruster_current  *  AUV_oceancurrent_x 

*  sin_psi 

+  k_thruster_current  *  AUV_oceancurrent_y 

*  cos_psi 

+  k_sway_hover  *  side_range_rate; 


/*  Extend  time  till  next  command  if  not  at  hover  point  yet  */ 
if  { range_from_recovery__pt  >  0.0) 

{ 

/*  still  not  at  the  recovery  point  */ 

if  (TRACE  &&  DISPLAYSCREEN)  printf {" [RECOVERY  cylinder  test]\n"); 

/*  continue  until  recoverpt  reached  without  further  script  orders  */ 
time_next_command  =  t  +  2.0  *  dt; 

} 


if  (TRACE 
{ 

printf 

printf 

printf 

printf 

printf 

printf 

printf 

printf 

printf 

printf 

printf 


&&  DISPLAYSCREEN) 


("[COMPUTED  RECOVERY  PARAMETERS  AND  CONTROLS] \n" ) ; 


("[Range  From  Final 
("[Range  From  Right 
("[Range  From  LEFT 
("[Range  Error 
("[Side  Range  Rate 
("fstbd  Propeller 
("[Port  Propeller 
("[AUV  Bow  Lateral 
("[AUV  Stern  Lateral 


[End  compute_recovery__controls]  \n"  )  ; 


Pt  %7 . 31f  ]  \n" ,  range_f  rom_recovery_pt) ; 
%7 . 31f  ]  \n",  range_from_right)  ; 

%7 .31f] \n",range_from_left)  ; 

%7 , 31f ] \n", side_range_error) ; 

%7 . 31f ] \n", side_range_rate) ; 

%7 . 31  f]  \n" ,  stbd_rpm_command)  ; 

%7 .31f] \n",port_rpm_command) ; 

%7-31f] \n",AUV_bow_lateral) ; 

%7 . 31f  ]  \n"  ,AUV_s template ral)  ; 
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} 

return;  /*  end  compute_recovery__controls  ()  */ 


/********************************************★******★***********************/ 

void  compute_target_controls  () 

{ 

static  int  waiting_for__tgt_update  =  FALSE; 

static  double  cos_tgt_brg  =  0.0, 
sin_tgt_brg  =  0.0, 
cos_tgt_brg_cind  =  0.0, 
sin_tgt_brg_cmd  =0.0, 
coinmanded_psi_tgt  =  0.0; 

double  distance_term_factor  =  1.0; 

if  (TRACE  ScSc  DISPLAYSCREEN) 

printf  ("[Begin  coinpute_target_controls]  \n" )  ; 

if  ( (new_target_update)  I  I  (NEWTARGETSTATION) ) 

{ 

cos_tgt_brg  =  cos  (radians  ( target_bearing) ) , 

sin_tgt_brg  =  sin  (radians  ( target_bearing) ) , 

cos_tgt_brg_cmd  =  cos  (radians  (normalize 

(180,0  +  target_bearing_command) ) ) , 
sin_tgt_brg_cind  =  sin  (radians  (normalize 

(180.0  +  targe t_bearing_command) ) ) ; 

/*  Compute  World  Space  Location  of  Station  Point  */ 
x_command  =  x 

+  cos_tgt_brg  *  targe t_range 
+  cos_tgt_brg_cmd  *  targe t_range_command; 
y_command  =  y 

+  sin__tgt_brg  *  target_range 
+  sin_tgt_brg_cmd  *  target_range_cominand; 

if  (NEWTARGETSTATION) 

commanded_psi_tgt  =  psi__command; 

NEWTARGETSTATION  =  FALSE; 

if  (new_target_update)  waiting_for_tgt_update  =  FALSE; 
new_targGt_update  =  FALSE; 

} 

if  (waiting_for_tgt_update  ==  FALSE) 

{ 

if  (TARGETPOINTING) 

psi_command_tgt  =  degrees  (atan22  (  (targe t_y  -  y)  ,  (targe t_x  -  x)  ) )  ; 
else  psi_command_tgt  =  commanded_psi__tgt ; 

/*  if  it  has  been  a  while  since  the  last  target  update,  */ 

/*  wait  here  for  next  one  */ 

if  ( (waiting_for_tgt_update  ==  FALSE)  && 

( ( (TARGETEDGETRACK)  &&  ( t ime_last_target_upda te  +  5.0  <=  t))  M 
( (TARGETEDGETRACK  ==  FALSE)  &&  ( t ime_last_target_upda te  +  7.5  <=  t)))) 

if  (TRUE  &&  DISPLAYSCREEN) 

{ 

printf ("Hovering  Until  New  Target  UpdateXn") ; 
printf("X:  %5.11f  Y:  %5.11f  Psi:  %5.11f\n", 
x_coramand,  y_command,  psi_cominand_hover)  ; 

x_command  =  x; 

y_command  =  y; 

psi_coinmand_hover  =  psi; 
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waiting_for_tgt_update  =  TRUE; 

) 

/*  Re-calculate  waypoint  distance  and  depth  error  */ 

/*  to  account  for  possibly  moving  target  */ 

waypoint_distanc€  =  sqrt  (  (x  -  x_command)  *  (x  -  x_command) 

+ (y  ^  y_command)  *  (y  -  y_command) } ; 

/*  If  waiting  for  target  update,  use  hovercontrol  to  hold  posit  */ 
if  {waiting_f or_tgt_update) 

{ 

c  ompu  t  e_ho ve  r_c  on t  ro  1  s  ( )  ; 

if  (REPORTSTABLE)  time_next_coinmand  =  t  +  2.0  *  dt; 
return; 

} 

waypoint_angle  =  normalize  (degrees  (atan2z 

(Y-Command  -  y,  x_coinmand  -  x) ) )  ; 
track_angle  =:  normalize  (waypoint_angle  -  psi); 

along_track_distance  =  cos  (radians  (track^angle) ) 

*  way point_di stance; 

cross_track_distance  =  -  sin  (radians  (track_angle) ) 

*  waypoint_di stance; 

port_rpm_command  =  k_propeller_hover  *  along_track_di stance 

-  k_pr Opel  le recurrent  *  AUV_oceancurrent_x 

*  cos__psi 

-  k_propeller_current  *  AUV__oceancurrent_y 

*  sin_psi 

-  k_surge_hover  /  2.5  *  u;  /*  3.0  better?  */ 

stbd_rpm_command  =  port_rpm__coinmand; 

if  (TRACE  ScSc  DISPLAYSCREEN) 

{ 

print  f  (''\nTARGETCONTROL:  \n")  ; 

printf  ("Station  Point:  %5.11f  Degrees  at  %5.11f  Feet\n", 
target_bearing_coinmand,  target_range_command)  ; 
printf  ("psi^command  =  %5.1f,\n",  psi_cornmand_tgt)  ; 

printf  ("Current  Point:  %5.11f  Degrees  at  %5.11f  FeetXn", 
target_bearing, target_range) ; 
printf  ("Computed  Station  Point:  %5.11f  %5.11f  %5.11f\n", 
x_command,  y__coinmand,  2__command)  ; 
printf  ("waypoint_distance  =  %5.1f,  track_angle  =  %5.1f\n", 
waypoint_distance,  track_angle) ; 

printf  ("along_track_distance  =  %5.1f,  ",  along_track_di stance) 
printf  ("cross_track_di stance  =  %5.1f\n",  cross_track_distance) 
printf  ("port_rpm  &  stbd^rpm  =  %5.1f\n",  port_rpm)  ; 


AUV_bow_lateral  =  -  (  -  k_thruster_psi  /  3.0  * 

normalize2  (psi  -  psi_coiranand_tgt) 

-  k__thruster_r  *  r) 

+  k_thruster_hover  /  1.5 

*  cross_track_di stance 

+  k_sway_hover  /  2.0  *  v  /*  3.0  better?  */ 

-  k_thruster_current  *  AUV_oceancurrent_x 

*  sin_psi 

+  k_thruster_current  *  AUV_oceancurrent_jY 

*  cos_psi; 

AUV_stern_lateral  =  (  -  k_thruster_psi  /  3.0  * 

normalize2  (psi  -  psi_command_tgt) 
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-  k__thruster_r  *  r) 

+  k_thruster__hover  /  1.5 

*  cross_track__distance 

+  k_sway_hover  /2.0*v  /*3.0  better?  */ 

-  k_thruster_current  *  AUV_oceancurrent_x 

*  sin_psi 

+  k_thruster„current  *  AUV_oceancurrent_y 

*  cos_^si; 


depth_error  =  (z^coiranand  -  z_kal); 

/*  constrain  depth_error  to  +-15.0  feet  to  prevent  going  vertical  */ 

/*  and  enable  stable  pitch  angle  even  on  large  depth  changes  */ 

clamp  {&  depth_error,  -15.0,  15.0,  "depth__error'' ) ;  /*  feet  */ 

compute_vertical_thrusters  {) ; 

/*  If  we  are  not  at  the  station  point  yet,  continue  */ 
if  ( (REPORTSTABLE)  && 

{ (time_last_target_update  +  1.0  <  t)  M 
(waypoint_distance  >0.5)  II 

(fabs  (normalize?  (psi  -  psi_command_tgt ) )  >  5.0))) 

{ 

time_next_coinmand  =  t  +  2.0  *  dt; 

} 

else 

{ 

if  {(TACTICAL)  &&  (REPORTSTABLE)) 

{■ 

strcpy  (buffer,  "STABLE  TARGET_STATION" ) ; 
if  (DISPLAYSCREEN)  printf  ("\n[%s]\n",  buffer); 
send_buf fer_to_tactical_socket  () ; 

} 

REPORTSTABLE  =  FALSE; 

} 

if  (TRACE  &&  DISPLAYSCREEN) 

printf  {"[End  compute_target_controls] \n" ) ; 

return;  /*  end  compute__target_controls  ()  */ 

} 

/******★************************★******************************************* ^ 

void  compute_waypoint_controls  () 

{ 

if  (T^CE  &&  DISPLAYSCREEN) 

printf  (" [begin  compute_waypoint_controls] \n") ; 

if  ( (port_rpm_command  <  200.0)  il  (stbd_rpiTi_command  <  200)) 

{ 

if  (TRACE  ScSc  DISPLAYSCREEN) 

printf  (" [WAYPOINTCONTROL  rpm  too  low,  reset  to  400.0] \n"); 
port_rpm_coiranand  =  400.0;  /*  boost  it  higher,  200-400  are  OK  */ 
stbd_rpm_coinmand  =  400.0; 

} 

waypoint_angle  =  atan2z  (y_command  -  y  +  AUV_oceancurrent_y  *  dt, 

x_command  -  x  +  AUV_oceancurrent_x  *  dt) ; 
waypoint_angle  =  normalize  (degrees  (waypoint_angle) )  ; 
psi_command  =  waypoint_angle; 

i f  (THRUSTERCONTROL ) 

compute__lateral_thrusters  ( )  ; 

/*  If  the  auv  is  closer  to  the  waypoint  than  this  value,  there  */ 
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/*  is  a  danger  that  it  could  enter  a  death  spiral  */ 

death_spiral_radius  =  fabs  (sin  (radians  (normalize2 

(waypoint_angle  -  psi)))) 
*  { rpm  /  700.0)  *  15.0; 


if  (TRACE  ScSc  DISPLAYSCREEN) 

{ 

printf  ("death_spiral_radius  =  %5.1f,  ",  death_spiral_radius) ; 
printf  ( "WAYPOINTCONTROL  psi_coiTimand  =  %5.1f,  ",  psi_command)  ; 
printf  ("X  =  %5.1f,  y  =  %5.1f\n",  x,  y)  ; 

} 


/*  Waypoint  not  reached,  continue  */ 

if  ( (FOLLOWWAYPOINTMODE)  &&  { HOVERCONTROL  ==  FALSE)  && 

(detect_death__spiral  (FALSE)  ==  FALSE)  &&  /*  check  it  */ 

( { (waypoint_di stance  >  standoff_di stance)  && 

(waypoint_di stance  >  death_,spiral_radius) )  I  I 
(fabs  (depth_error)  >  standof f_di stance) ) ) 

{ 

if  (TRACE  &&  DISPLAYSCREEN) 

printf  ("\n[ FOLLOWWAYPOINTMODE  cylinder  test]"); 

/*  continue  until  WAYPOINT  reached  without  further  script  orders  */ 
time_next_cominand  =  t  +  2.0  *  dt; 

} 


/*  Waypoint  Reached  */ 

else  if  ((fabs  (depth_error)  <=  standof f_di stance)  && 

( (waypoint_distance  <=  standof f_di stance)  M 
(waypoint_distance  <=  death_spiral_radius)  I  1 
(detect_death_spiral  (FALSE))))  /*  check  it  */ 

( 


WAYPOINTCONTROL  =  FALSE; 

FOLLOWWAYPOINTMODE  =  FALSE; 

if  (TRACE  &&  DISPLAYSCREEN) 

printf  ("\n [FOLLOWWAYPOINTMODE  success,  WAYPOINT  reached]"); 
if  (HOVERCONTROL  ==  FALSE)  DEATH_SPIRAL_RESET  =  TRUE; 

/*  report  .STABLE  to  tactical  level  once  waypoint  received  */ 

if  ((TACTICAL)  &&  (REPORTSTABLE)  &&  (HOVERCONTROL  ==  FALSE) 

ScSc  (GPSFIXINPROGRESS  ==  FALSE)  ) 

{ 

REPORTSTABLE  =  FALSE; 
strcpy  (buffer,  "STABLE  WAYPOINT") ; 
if  (DISPLAYSCREEN)  printf  ("\n[%s]\n",  buffer); 
send_buffer_to_tactical_socket  ();  /*  message  */ 

} 

} 

if  (TRACE  ScSc  DISPLAYSCREEN) 

printf  ("[end  compute_waypoint_controls] \n" ) ; 

return;  /*  end  compute_waypoint_controls  ()  */ 

} 

/*************************************************************************** y 

void  compute_lateral_controls  () 

{ 

if  (TRACE  ScSc  DISPLAYSCREEN) 

printf  ("[begin  compute_lateral_controls] \n" ) ; 

compute_vGrtical_thrusters  {); 

AUV_bow_lateral  =  -  k_thruster_lateral  *  lateral_command; 
AUV_stern_lateral  =  AUV_bow_lateral  ; 
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psi_coiranand  =  psi; 

if  (TRACE  &&  DISPLAYSCREEN) 

print f  ("  [end  coTnpute_lateral_controls]  \n" ) ; 


return; 


} 

/******************************** **********^^********^*^*^^^^*^*^^^^^^^^^^^^^ 

void  coinpute_rotate_controls  () 

{ 

if  (TRACE  ScSc  DISPLAYSCREEN) 

printf  ("  [begin  coinpute_rotate_controls]  \n")  ; 

compute_vertical_thrusters  { ) ; 


if  (TRACE  ScSc  DISPLAYSCREEN) 

printf  {" (ROTATECONTROL  ==  TRUE)\n"); 


AUV_stern_lateral  =  k_thruster_rotate  *  rotate_cornniand; 
-^UV__bow_lateral  =  “AUV__stern_lateral ;  /*  negative  */ 


if  (TRACE  &Sc  DISPLAYSCREEN) 

printf  ("[end  compute_rotate_controls] \n" ) ; 


It****************  j 


void  compute_lateral_thnisters  {) 

{ 

if  (TRACE  &&  DISPLAYSCREEN) 

printf  ("  [begin  coinpute_lateral_thrusters]  \n")  ; 


coinpute_vertical_thrusters  ()  ; 


AUV_stern_lateral  =  -  k_thruster_psi  *  nonnalize2  (psi  -  psi_coiranand) 
“  k_thruster__r  *  r; 

AUV_bow__lateral  =  -  AUV_stern_lateral;  /*  negative  */ 

if  (TRACE  &&  DISPLAYSCREEN) 

printf  ("[end  compute_lateral_thrusters] \n" ) ; 


return; 

} 

/************************************** **^ 
void  compute_vertical_thrusCers  () 


{ 


static  double  depth_error_integral  =  0.0; 

if  (TRACE  &&  DISPLAYSCREEN) 

printf  ("(begin  coinpute_vertical_thrusters] \n") ; 

if  ( ( INTEGRALDEPTHCONTROL  ==  FALSE)  &&  (t  >=  tinie_int_control  on)) 
INTEGRALDEPTHCONTROL  =  TRUE;  “ 


/*  Only  use  error  in  INTEGRAL  CONTROL  only,  clamp  to  avoid  saturation  */ 
deptli_error_integral  =  (  depth_error_integral 

+  (z_kal  -  z_command)  *  TIMESTEP) 

*  INTEGRALDEPTHCONTROL; 

clamp  (&depth_error_integral,  -2.0,  2.0,  "depth_error_integral" ) ; 

if  (TRACE  &&  DISPLAYSCREEN) 

{ 

printf  ("[z_)cal  =  %5 .31f  ]  \n",  z_)cal)  ; 

printf  ("[z_dot_kal  =  %5 . 31f ] \n" , z_dot_kal) ; 

printf  ("[z_command  =  %5 .31f ] \n" , z_command) ; 

printf  ("(depth  error  =  %5 .31f ] \n" , z_kal  -  z_command) ; 

printf  ("(Integral  Depth  Error  =  %5.31f]\n",  depth_error_integral) ; 


175 


} 


AUV_bow_verti cal  =  k_thruster_z  *  (z_kal  z^command) 

“  k_thruster_w  *  z_dot_kal 
“  5.0  *  depth_error_integrar; 

AUV_stern_vertical  =  AUV_bow_vertical ; 

if  (TRACE  &&  DISPLAYSCREEN) 

printf  ("[Pre  Pitch  Thruster  Values:  %5 .31f ] \n" ,AUV_bow_vertical) ; 

/*  include  pitch  control  when  hovering  or  tracking  a  target  */ 

/*  requires  reverification  in  water,  apparent  sign  error  problem.., 
if  { (HOVERCONTROL)  II  ( TARGETCONTROL )  I  I  ( RECOVER YCONTROL ) ) 

AUV_bow_vertical  +=  -  k_thruster_theta  *  (theta  -  theta_command) 

-  k_thruster_theta  *  q  *  (2.0); 

AUV_stern_vertical  +=  k_thruster_theta  *  (theta  -  theta^command) 

+  k_thruster_theta  *  q  *  (2.0); 

*/ 

return; 

} 

/*********★**** ************************************************ ***********^*^ 

void  compute_fin_controls  () 

{ 

if  (TRACE  DISPLAYSCREEN) 

printf  ("[begin  compute_f in_controls] \n" ) ; 

/*  Report  Stable  Course  to  Tactical  Level  if  Required  */ 
if  { (REPORTSTABLE)  && 

(fabs  (normalize2  (psi  -  psi_command) )  <  2.5  /*  degrees  */  )) 

if  ((TACTICAL)  ScSc  (HOVERCONTROL  ==  FALSE)  && 

(WAYPOINTCONTROL  ==  FALSE)) 

{ 

REPORTSTABLE  =  FALSE; 
strcpy  (buffer,  "STABLE  COURSE" ) ; 
if  (DISPLAYSCREEN)  printf  ("\n[%s]\n",  buffer); 
send_buffer_to_tactical_socket  ();  /*  message  */ 


} 

/*  Simplified  PD  rudders /planes  control  rules;  —  -  -  —  -  —  _*/ 

/*  calculate  rudders  - 


delta_rudder  =  k_psi  *  normalize2  (psi  ~  psi^command) 

+  (k_r  *  r)  +  (k_v  *  v)  ; 

/*  tanh  not  provided  under  OS-9  C,  added  at  end  of  this  program  */ 

/*  tanh  was  change  to  dtanh  toconserve  time  in  approximation  */ 

i f  ( SL ID INGMODECOURS  E ) 

{ 

sigma  =  k_sigma_r  *  r  +  k_sigma_psi  *  normali2e2  (psi  -  psi__command) ; 
delta_rudder  =  (3.1403  *  r)  +  81.9712  *  eta_steering  *  dtanh  (sigma); 


/*  reduce  ordered  rudder  if  excessive  roll  occurs,  may  work  for  many  UUVs*/ 

delta_rudder  =  delta_rudder  *  cos__phi  *  cos_phi; 

if  (TRUE  &&  DISPLAYSCREEN  &&:  (cos  phi  *  cos  phi  <  0.98)) 

{ 
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} 


printf  ( "\nr udder/p lanes  reduction  factor  due  to  roll  phi  =  %6.3f\n", 
cos  j>hi  *  cos_phi )  ; 


/*  calculate  planes  -  --  --  --  -- 
delta_planes  =  (k_z  *  depth_error} 


+  (k_theta  *  theta)  +  (k_q  *  q)  -  (k_w  *  z_dot_kal) ; 

if  (TRACE)  printf  ("deltajlanes=:%5  .  Ilf \n"  .  delta^planes)  ; 
if  (TRACE)  printf  { "depth_error  =:%5.11f,  product=%5  .  Ilf  \n"  , 


depth_error, 

if  (TRACE)  printf  ("theta 
theta, 

if  (TRACE)  printf  ("q 
Q/ 

if  (TRACE)  printf  ( " z_cominand 
z_conimand, 

if  (TRACE)  printf  ("z_dot_kal 
2-Ciot_kal, 


k_z  *  depth_error)  ; 

=%5 . Ilf ,  product=%5 , Ilf \n" , 
k_theta  *  theta) ; 

=%5.11f,  product=%5.11f\n", 
k_q  *  q) ; 

=%5.11f,  z_kal  =%5.11f\n", 
z  ka 1 )  * 

=%5.11f'  product=%5.11f\n", 
-k_w  *  z_dot_kal); 


/*  temporary  fix  to  incorrect  delta_planes  polarity  in  boat 
if  (LOCATIONLAB  ==  FALSE) 

{ 

if  (TRACE  ScSc  DISPLAYSCREEN) 

printf  {"[reversing  polarity  delta_planes,  delta_rudder ] " ) ; 
delta_j)lanes  =  -  delta_planes; 
delta_rudder  =  -  delta_rudder; 


*/ 


V 


/ 


*  reduce  ordered  planes  if  excessive  roll  occurs,  may  work  for  many  UUVs*/ 
delta__planes  =  delta  planes  *  cos  phi  *  cos  phi ; 

/*  Dead  stick  means  no  open  loop  control  of  rudders /planes  ------*/ 

if  (DEADSTICKRUDDER) 

{ 

delta_rudder  =  rudder_command; 

} 

i f  ( DEADSTICKPLANES ) 

{ 

delta_planes  =  planes_command; 


} 


/*  constrain  planes  &  rudder  orders  +/-  22.5  degrees  -  do  not  normalize!  */ 
clamp  (&  delta_rudder,  -22.5,  22.5,  "del ta_r udder" ) ;  /*  degrees  */ 

if  (fabs  (rpm)  <  400.0) 

clamp  (&  delta^planes,  -22.5,  22.5,  "delta_planes" )  ;  /*  low  speed  */ 
else  if  (fabs  (rpm)  <  500.0) 

clamp  (&  del ta_planes,  -15.0,  15.0,  "delta_planes" ) ;  /*medium  speed*/ 
else  if  (fabs  (rpm)  <  700.0) 

clamp  (&  del ta^jplanes,  -10.0,  10.0,  "delta  planes") ;  /*  high  speed  */ 
else  clamp  (&  delta__planes,  -5.0,  5.0,  "delta_planes" ) ;  /*super  speed  */ 

if  (TRACE  ScSc  DISPLAYSCREEN) 

printf  ("[end  compute_fin_controls] \n" ) ; 

return;  /*  end  compute_f in_controls  ()  */ 

/****************************************** ********^**^^^*^*^^^^^^^^^^^^^^^^  ^ 

/*  The  following  four  functions  were  added  on  12  Dec  95  */ 

/*  They  are  from  Dave  Marco's  execution  code  and  are  used  */ 

/*  for  speed  control  of  the  port  propel lors  */ 
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int  port_speed_control (n_com) 
double  n_coin; 


{ 


double  Kni_ls  =  0.6589/ 


e_n,  v_ls_spc/ 
eta_ls  =  10.0, 
phi_ls  =  5.0; 


/*  revolutions  per  second  */ 


if (fabs (n_com)  <  0,25)  Int_ls  =  0.0; 


e_n  =  n__com  -  read_port_motor_rpm  {)  /  60.0; 

Int„ls  =  Int_ls  +  dtanh(e_n/phi_ls) *dt; 

v_ls_spc  =  (1 . 0/Kin_ls)  *  {n_coin  +  eta_ls*Int_ls)  ; 

v_dls  =  (int)  { (1023 .0/48. 0) * (v_ls_spc)  +  511.5); 

if(v_dls  <  0  )  v_dls  =  0; 
if(v_dls  >  1023  )  v_dls  =  1023; 
return (v_dls) ; 

}  /*  end  port__speed_control  ()  */ 


int  stbd__speed_control  (n_com) 
double  n_com; 


double  Km_rs  =  0.'6156/ 


/*  revolutions  per  second  */ 


e_n/  v_rs_spC/ 
eta__rs  =  10.0, 
phi_rs  =  5.0; 

if  (fabs  (n_coin)  <  0.25)  Inters  =  0.0; 

e_n  =  n^com  -  read_stbd_iaotor_rpm( )  /  60.0; 

Int_rs  =  Inters  +  dtanh  (e_n/phi_rs)  *dt; 

v_rs_spc  =  (1 . 0/Kin_rs)  *  (n_com  +  eta_rs  *  Inters )  ; 

v_drs  =  (int)  ( (1023 .0/48 .0) * (v_rs_spc)  +  511.5); 

if(v_drs  <  0  )  v_drs  =  0; 

if(v_drs  >  1023  )  v_drs  =  1023; 
return (v_drs) ; 

’}  /*  end  stbd_speed__control  {)  * / 

/************************************★************************************** ^ 

/*  -  VERIFIED  MATCH  CURRENT/OPERATIONAL  MARCO  AUV  HARDWARE /SOFTWARE  -  */ 

double  read_depth  ()  /*  Return  depth  in  FEET  */ 

{ 

int  val  =  0; 

double  new_z  =  0.0;  /*  zz  in  dave's  execf.c  code  */ 

double  z_offset  =  0.0; 

if  (TRACE  &&  DISPLAYSCREEN)  printf  ("\n[start  read^depth  01"); 

if  (LOCATIONLAB  &&  DEADRECKON) 

{ 

new_z  =  2_conimand; 

} 

else  if  (LOCATIONLAB) 

{ 
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new_z  =  z;  /*  no  change,  use  virtual  world  value  */ 

} 

else  /*  in-water  */ 

{ 

/*  val  =  adcl{DEPTH_CELL_CH) ;  */  /*  Channel  7  */ 

/*  0.0728  =  0.0182*4.0  */ 

/*  Since  A/D  now  has  0-1023  range  instead  of  0-4095  */ 

/*  new_z  =  0.0728*  (  (double)  (val  -  z_val0)}  +  z_offset;  */ 

/*  adc2  card  has  0  -  4095  resolution  */ 
val  =  get_adc2  (DEPTH_CELL_CH,  0); 

new_z  =  0.0182*(  (double)  (val  -  z^valO))  +  z^offset; 

/*  Calibration  for  Signal  Amp  */ 

/*new__z  =  0.0034285* (  (double)  (z_val0  -  val))  +  z_offset;*/ 

} 

if  (TRACE  ScSc  DISPLAYSCREEN) 

printf  ("\n[ finish  read_depth  (),  returns  %5.3f]\n",  new_z); 
return  (new_z  +  depth_cell_bias) ; 

}  /*  end  read__depth  ()  */ 

/*************************************************************************** ^ 

/*  -  VERIFIED  MATCH  CURRENT/OPERATIONAL  MARCO  AUV  HARDWARE /SOFTWARE  -  */ 

double  read_psi  ()  /*  return  psi  in  degrees  */ 

unsigned  short  psi_bit; 

int  psi_bit„int,psi_bit_old_int, delta_psi_bit; 
double  angle, tpi; 
double  pi  =  3.1415927; 

if  (TRACE  &&  DISPLAYSCREEN)  printf  ("[start  read_psi  ()]\n"); 

if  (LOCATIONLAB  &&  DEADRECKON) 

{ 

angle  =  psi^command; 

} 

else  if  (LOCATIONLAB) 

{ 

psi  =  psi;  /*  no  change,  use  virtual  world  value  */ 
angle  =  psi;  /*  set  up  for  function  return  */ 

} 

else  /*  in-water  */ 

{ 

psi_bit  =  Read_PortAB(0xFFF00700) ; 
psi__bit  Sc-  0x3 FFF; 
psi_bit_int  =  psi_bit; 
psi_bit_old_int  =  psi_bit_old; 

delta__psi_bit  =  psi_bit__int  -  psi_bit_old_int; 
psi_bit_old  =  psi_bit; 

if (abs (delta_psi_bit)  >  10000) 

{ 

wrap_count  =  wrap_count  -  delta_psi_bit/abs  (delta_psi_bit )  ; 


angle  =  start^si  +  degrees  {  (read_heading  {)  - 

dg_offset  +  2.0*pi* ( (double)  wrap_count) ) ) ; 

if (fabs (angle)  <  0.0001)  angle  =  0.0; 

/*printf("%f  %f  %f  %d  %d\n", 

angle,  read_heading  ()  ,  dg_of  f set , wrap_count,psi__bit)  ;  */ 
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} 


if  (TRACE  ScSc  DISPLAYSCREEN) 

printf  {"[finish  read_psi  {)  returns  %5.3f]\n",  angle); 
return  (normalize  (angle)); 

}  /*  end  read_j}si  ()  */ 

/********************************************** ****************************^^ 

/* -  VERIFIED  MATCH  CURRENT/OPERATIONAL  MARCO  AUV  HARDWARE /SOFTWARE - */ 

double  read_roll_rate_gyro  {)  /*  Return  roll  rate  in  DEGREES /SEC 

*/ 

{ 

int  val  ; 
double  rate; 

if  (TRACE  ScSc  DISPLAYSCREEN)  printf  ("[start  read_roll_rate _gyro  ()]\n"); 

if  (LOCATIONLAB) 

{ 

rate  =  p;  /*  no  change,  use  virtual  world  value  */ 
if  (fabs  (rate)  <  0.0001)  rate  =  6.0; 

} 

else  /*  in-water  */ 

{ 

val  =  get_adc2 (ROLL_RATE_CH, 0) ; 

/*  Next  two  lines  from  old  method  */ 

/*val  =  val  »  2;*/  /*  Quick  fix  for  new  res  */ 

/*rate  =  (roll_rate_0/3 . 2113  -  . 31062*val) /57 . 295779 ; */ 
rate  =  degrees  (0 . 07785* (roll_rate_0  -  val ) /57 . 295779) ; 
if (fabs (rate)  <  0.0001)  rate  =  0.0; 

} 

rate  =  normalize2  (rate); 
if  (TRACE  ScSc  DISPLAYSCREEN) 

printf  ("[finish  read_roll_rate_gyro  ()  returns  %5.3f]\n",  rate) ; 
return  (rate); 

}  /*  end  read_roll_rate_gyro  ()  */ 

/***************************************************************************/ 

/* -  VERIFIED  MATCH  CURRENT /OPERATIONAL  MARCO  AUV  HARDWARE /SOFTWARE - */ 

double  read_pitch_rate_gyro  ()  /*  Return  pitch  rate  in  DEGREES/SEC  */ 

int  val  =  0; 
double  rate; 

if  (TRACE  ScSc  DISPLAYSCREEN)  printf  ("[start  read_pitch_rate_gyro  {)]\n"); 
if  (LOCATIONLAB) 

{ 

rate  =  q;  /*  no  change,  use  virtual  world  value  */ 
if  (fabs  (rate)  <  0.0001)  rate  =  0.0; 

} 

else  /*  in-water  */ 

( 

val  =  get_adc2 (PITCH_RATE_CH,0) ; 

/*  Next  two  lines  from  old  method  */ 

/*val  =  val  »  2;*/  /*  Quick  fix  for  new  res  */ 

/*rate  =  (pi tch_rate_0/13 . 69399  -  .0730001*val) /57 .295779; */ 
rate  =  degrees  (0 . 01825* (pi tch_rate_0  -  val) /57 .295779) ; 
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if (fabs{rate)  <  0.0001)  rate  =  0.0; 

} 

rate  =  normalize2  (rate) ; 
if  (TRACE  &&  DISPLAYSCREEN) 

printf  ("[finish  read_pitch_rate_gYro  ()  returns  %5.3f]\n",  rate); 
return  (rate); 

}  /*  end  read_pitch_rate_gyro  ()  */ 

/************************************,********************, ***,^^^^^^^^^^^^^^ 

/*  -  VERIFIED  MATCH  CURRENT/OPERATIONAL  MARCO  AUV  HARDWARE/SOFTWARE  -  */ 

double  read_yaw_rate_gyro  ()  /*  Return  yaw  rate  in  DEGREES/SEC  */ 

int  val  =  0; 
double  rate; 


if  (TRACE  &&  DISPLAYSCREEN)  printf  ("[start  read..vaw  rate  ovro  ()]\n*); 
if  (LOCATIONLAB) 

{ 

rate  =  r;  /*  no  change,  use  virtual  world  value  */ 
if  (fabs  (rate)  <  0.0001)  rate  =  0.0; 

} 

else  /*  in-water  */ 

{ 

/*  Below  for  add  Card  */ 

/*val  =  add (YAW_RATE_CH) ; */  /*  Channel  10  */ 

/*rate  =  2.18*  {  ((double)  yaw_rate_0) /13 .653216  - 

0.0732362*  (  (double)  val)  ) 757 .295779; */ 

val  =  get_adc2 (YAW_RATE_CH,0) ; 

/*  Next  two  lines  from  old  method  */ 

/*val  =  val  »  2;*/  /*  Quick  fix  for  new  res  */ 

/*rate  =  2 .78* (yaw_rate_0/13 . 653216  -  .0732362*val)  757 .295779;  */ 
rate  =  degrees  (0.0509* (yaw_rate_0  -  val) 757 .295779) ; 
if (fabs (rate)  <  0.0001)  rate  =  0.0; 


rate  =  normalize2  (rate) ; 
if  (TRACE  &&  DISPLAYSCREEN) 

printf  ("[finish  read_yaw_rate_gyro  ()  returns  %5.3f)\n",  rate); 
return  (rate); 

}  7*  end  read_yaw_rate_gyro  ()  *7 

/**************************************^,^c*****i,**^:i,^,**i,^,^,t****i,i,i,i,i,*i,^,^,^,^,^,^,^,/ 

/*  -  VERIFIED  MATCH  CURRENT70PERATI0NAL  MARCO  AUV  HARDWARE 7S0FTWARE  -  *7 

double  read_port_motor_rpm  ()  7*  Reads  rpm  from  PORT_PROP  *7 

int  pulse; 
double  local_port_rpm; 

if  (TRACE  &&  DISPLAYSCREEN)  printf  ("[start  read_port_motor_rpm  ()]\n"); 
local_port_rpm  =  read_motor  (PORT_PROP); 
if  (TRACE  USc  DISPLAYSCREEN) 

printf  ("[finish  read_port_motor_rpm  ()  returns  %5.3f]\n", 
local_port_rpm) ; 
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return  (local_port_rpm) ; 

}  /*  end  read^port^inotor^rpin  ()  */ 

/************** *************************************************************^ 

/*  VERIFIED  MATCH  CURRENT/OPERATIONAL  MARCO  AUV  HARDWARE /SOFTWARE  — -  */ 

double  read_stbd_motor_rpm  {)  /*  Reads  rpm  from  STBD_PROP  */ 

{ 

int  pulse; 
double  local_stbd_rpin; 

if  (TRACE  &&  DISPLAYSCREEN)  printf  ("[start  read_stbd_motor_rpm  ()]\n"); 
local_stbd_rpm  =  read_motor  (STBD_PROP) ; 
if  (TRACE  ScSc  DISPLAYSCREEN) 

printf  ("[finish  read_stbd_motor__rpm  ()  returns  %5.3f]\n", 
local_stbd_rpm) ; 

return  ( local__stbd_rpm)  ; 

}  /*  end  read_stbd_motor_rpm  ()  */ 

/***************★****************************************** *****************^ 
/*  -  VERIFIED  MATCH  CURRENT/OPERATIONAL  MARCO  AUV  HARDWARE /SOFTWARE  */ 


double  read_motor  (motor)  /*  Read  rpm  from  single  propellor  or  thruster  */ 

int  motor; 


motor 

=  0 

Left 

Propeller  . 

PORT  PROP 

RPM 

1 

Right 

Propeller 

STBD  PROP 

RPM 

2 

Bow 

Vertical  Thruster 

BOW  VERTICAL 

volts 

3 

Bow 

Lateral  Thruster 

STERN^VERTICAL 

volts 

4 

Stern 

Vertical  Thruster 

BOW  LATERAL 

volts 

int  count; 

5 

Stern 

Lateral  Thruster 

STERN  LATERAL 

volts  */ 

double  freq, rps; 

unsigned  char  lobyte,hibyte; 


if  (TRACE  &&  DISPLAYSCREEN)  printf  ("[start  read_motor  ()]\n"); 


if  (LOCATIONLAB  ==  FALSE)  /*  in  water  */ 

{ 

swi tch (motor) 

{ 

case  PORT_PROP: 

write_timla (3, tim_la_control_reg, 17) ; /*  Sel  Cntr  1  HOLD  Reg.  Card  3  */ 
lobyte  =  read__timlacl  (3,  tim_la_data_reg)  ; 
hibyte  =  read_timlacl  (3,  tim_la_data_reg)  ; 
count  =  (int)  (256*hibyte)  +  (int)  lobyte; 

if(v_dls  <  512  )  count  =  -count;  /*  Account  for  Direction  of  Rot.  */ 
break; 


case  STBD_PROP: 

write_timla(3,  tim_la_control_reg,18)  ;/*  Sel  Cntr  2  HOLD  Reg.  Card  3  */ 
lobyte  =  read_timlacl  (3,  tim_la_data_reg) ; 
hibyte  =  read_timlacl  (3 ,  tim_la_data_reg)  ; 
count  =  (int)  (256*hibyte)  +  (int)  lobyte; 

if(v_drs  <  512  )  count  =’ -count;  /*  Account  for  Direction  of  Rot.  */ 
break; 


case  BOW^VERTICAL : 

write_timla(2,  tim_la_control_reg,17)  ;/*  Sel  Cntr  1  HOLD  Reg.  Card  2  */ 
lobyte  =  read_timlacl  (2 ,  tim_la_data_reg)  ; 
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hibyte  =  read_timlacl  (2 ,  tiin_la_data__reg)  ; 
count  =  (int)  {256*hibyte)  +  (int)  lobyte; 

if{v_dbvt  <  512  )  count  =  -count;  /*  Account  for  Direction  of  Rot.  */ 
break; 

case  STERN_VERTICAL : 

write_timla{2, tiin_la_control_reg, 18) ;  /*Sel  Cntr  2  HOLD  Reg.  Card  2  */ 
lobyte  =  read__timlacl  {2,  tiin_la__data_reg)  ; 
hibyte  =  read_timlacl  (2 ,  tiin_la_data_reg)  ; 
count  =  (int)  (256*hibyte)  +  (int)  lobyte; 

if(v_dblt  <  512  )  count  =  -count;  /*  Account  for  Direction  of  Rot.  */ 
break; 

case  BOW_LATERAL: 

write_tiinla(2, tim_la_control_reg,19) ;  /*Sel  Cntr  3  HOLD  Reg.  Card  2  */ 
lobyte  =  read^timlacl  (2,  tim_la_data_reg)  ; 
hibyte  =  read__timlacl  (2 ,  tim__la_data_reg)  ; 
count  =  (int)  (256*hibyte)  +  (int)  lobyte; 

if(v_dsvt  <  512  )  count  =  -count;  /*  Account  for  Direction  of  Rot.  */ 
break; 

case  STERN_LATERAL: 

write_timla(2,  tiin_la__control_reg,20)  ;  /*Sel  Cntr  4  HOLD  Reg.  Card  2  */ 
lobyte  =  read^timlacl  (2,  tim_la_data_reg)  ; 
hibyte  =  read^timlacl  (2,  tiin_la_data_reg)  ; 
count  =  (int)  (256*hibyte)  +  (int)  lobyte; 

if(v_dslt  <  512  )  count  =  -count;  /*  Account  for  Direction  of  Rot.  */ 
break; 

default: 

if  (DISPLAYSCREEN) 

printf  (" [read_motor  ()  error:  illegal  motor  value  (%d)]\n",  motor); 
break; 

} 

if (count  !=  0) 

{ 

freq  =  (1 . 0/count ) *4 . 0*pow ( 10 , 0, 6 . 0 ) ;  /*F1  (1  Mhz)  The  4.0  is  in  there  */ 

/*  as  a  scale  factor  from  God  */ 

} 

else 

{ 

/*  Sensor  Not  Counting  */ 
freq  =  0.0; 

} 

/*  500  Counts  Per  Rev  */ 

^s  =  (freg/500.0); 

if  ( (fabs(rps)'  <  1,0)  I!  (fabs(rps)  >  1000.0))  rps  =  0.0; 
if  (TRACE  &&  DISPLAYSCREEN) 

printf  ("[finish  read^motor  ()  returns  %5.3f]\n",  rps) ; 
return  (3p»s  *  60.0);  /*  convert  from  per-seconds  to  per-minutes  */ 
else  /*  LOCATIONLAB  ==  TRUE  */ 
return  (rpm); 

}  /*  end  read_motor  ()  */ 

/*********★**************************************************** ************^^ 

/*  -  VERIFIED  MATCH  CURRENT /OPERATIONAL  MARCO  AUV  HARDWARE /SOFTWARE  -  */ 

double  read_roll_angle  {)  /*  Return  roll  angle  in  DEGREES  */ 

int  val ; 
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double  angle; 

if  (TRACE  ScSc  DISPLAYSCREEN)  printf  {"[start  read_roll_angle  ()]\n"); 
if  (LOCATIONLAB) 

{ 

angle  =  phi;  /*  no  change,  use  virtual  world  value  */ 
if  (fabs  (angle)  <  0.0001)  angle  =  0.0; 

} 

else  /*  in-water  */ 

{ 

val  =  get_adc2  ( ROLL_ANGLE_CH , 0 ) ; 

/*  Next  three  lines  from  old  method  */ 

/*val  =  val  »  2;*/  /*  Quick  fix  for  new  res  */ 

/*  angle  =  {(516.578  -  val) /5 .7572) /57 .295779;  convert  to  radians  */ 
/*angle  =  {~.1737*val  +  . 1737*roll_0 ) /57 . 295779 ; */ 
angle  =  0 . 043425*  ( roll_0  -  val ) /•57 .295779; 
if  (fabs  (angle)  <  0.0001)  angle  =  0.0; 

) 

angle  =  normalize2  (angle); 
if  (TRACE  &&  DISPLAYSCREEN) 

printf  ("[finish  read_roll_angle  {)  returns  %5.3f]\n",  angle); 
return  (angle) ; 

} 

/*********************************:t**********************************-rfr******/ 

/*  VERIFIED  MATCH  CURRENT/ OPERATIONAL  MARCO  AUV  HARDWARE /SOFTWARE  — -  */ 

double  read_pitch_angle  ()  /*  Return  pitch  angle  in  DEGREES  */ 

{ 

int  val; 
double  angle; 

if  (TRACE  ScSc  DISPLAYSCREEN)  printf  {"[start  readmit ch^angle  ()]\n"); 
if  (LOCATIONLAB) 

{ 

angle  =  theta;  /*  no  change,  use  virtual  world  value  */ 
if  (fabs  (angle)  <  0.0001)  angle  =  0.0; 

} 

else  /*  in-water  */ 

{ 

val  =  get_adc2  {PITCH_ANGLE„CH, 0 ) ; 

/*  Next  three  lines  from  old  method  */ 

/*val  =:  val  »  2;*/  /*  Quick  fix  for  new  res  */ 

/*  angle  =  ((520.153  -  val ) /8 . 340) /57 . 295779;  convert  to  radians  */ 
/*angle  =  {{-.1199*val  +  . 1199*pitch_0) /57 .295779) ; */ 
angle  =  degrees  (0 .02997* (pitch_0  -  val ) /57 . 295779 ) ; 
if  (fabs  (angle)  <  0.0001)  angle  =  0.0; 

} 

angle  =  normalize2  (angle); 
if  (TRACE  ScSc  DISPLAYSCREEN) 

printf  ("[finish  read_pitch_angle  ()  returns  %5.3f]\n",  angle); 
return  (angle) ; 

} 

/***************************************************************************/ 

/*  ---  VERIFIED  MATCH  CURRENT/ OPERATIONAL  MARCO  AUV  HARDWARE /SOFTWARE  */ 

double  read_heading  () 
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/*  Return  heading  angle  with  respect  to  local  magnetic  north  in  radians 
from  directional  gyro  */ 

unsigned  short  dg_bit; 
double  angle ; 

if  (TRACE  ScSc  DISPLAYSCREEN)  printf  {"{start  read_heading  {)]\n"); 

if  (LOCATIONLAB  &&  (DEADRECKON  ==  FALSE)) 

{ 

angle  =  psi; 

if  (fabs  (angle)  <  0.0001)  angle  =  0.0; 

} 

else  if  (LOCATIONLAB  &&  (DEADRECKON) ) 

{ 

angle  =  psi_coinmand; 

if  (fabs  (angle)  <  0.0001)  angle  =  0.0; 

} 

else  /*  in-water  */ 

{ 

/*dg_bit  =  Read__PortAB(MFI_BASE)  ;*/ 

dg_bit  =  Read_PortAB(0xFFF00700)  ;  /*  why  not  a  #define  here?  ««  */ 

/*dg_bit  =  10000;*/ 

<^g_bit  &=  0x3FFF; 

angle  =  (3 . 8350e-4) *{ (double)  dg_bit); 

/*printf ("Angle  =  %f  %d\n" , angle, dg_bit) ; */ 

/*if (fabs (angle)  <  0.001)  angle  =  0.0;*/ 

} 

angle  =  normalize  (angle); 
if  (TRACE  &&  DISPLAYSCREEN) 

printf  ("[finish  read_heading  ()  returns  %5.3f]\n",  angle) ; 
return  (angle) ; 


/**********★*★★************************************************************* y 

double  read_speed  ()  /*  Filter  the  speed  signal  */ 

{ 

static  int  old_countl, old_count2 ; 
static  int  start  =  TRUE; 
int  county- 

unsigned  char  lobyte,hibyte; 
double  freq; 
double  avg_speed; 

if  (TRACE  &&  DISPLAYSCREEN) 

printf  ("[start  read_speed  (),  LOCATIONLAB=%d] \n" ,  LOCATIONLAB); 
if  (LOCATIONLAB) 

{ 

if  (TRACE  &&  DISPLAYSCREEN) 

printf  {"[finish  read__speed  ()  returns  %5.3f]\n",  speed); 

return  (speed);  /*  from  virtual  world-paddlewheel  speed  =  u  =  surge  */ 

else  if  (DEADRECKON) 

{ 

if  (TRACE  &&  DISPLAYSCREEN) 

printf  ("[finish  read_speed  {)  DEADRECKON  returns  "); 
avg_speed  =  (speed_per_rpm  *  (port_rpm  +  stbd_rpm)  /  2.0); 
if  (TRACE  &&  DISPLAYSCREEN) 

printf  ("%5.3f]\n",  avg_speed) ; 
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return  (avg_speed) ; 


} 

else 

{  /*  I  think  this  is  Dave's  speed  averaging  code  */ 

if (start) 

{ 

old_countl  =  0; 
old_count2  =  0; 
start  =  FALSE; 

} 

write^timla  (3,  t iin_la_control_reg,  19)  ; 
lobyte  =  read_timlacl  (3 ,  tim_la_data_reg)  ; 
hibyte  =  read__tiinlacl  (3 ,  tiTn_la_data_reg)  ; 
count  =  (int)  (256*hibyte)  +  (int)  lobyte; 

if { (old_countl  ==  count)  && 

(old__count2  ==  count)) 

{ 

old_countl  =  old_count2; 
old_count2  =  count; 
return (0.0) ; 

} 

old_countl  =  old_count2; 
old_count2  =  count; 


} 

if  (TRACE  ScSc  DISPLAYSCREEN) 

printf  ("[finish  read__speed  ()  returns  %5.3f]\n",  avg_speed) ; 

if (count  !=  0) 

{ 

freq  =  (1 . 0/ (2 . 0*count)  )  *  4.0  *  10000.0;  /*  F3  (10,000  Hz)  */ 
if  (freq  >=  17)  freq  =  freq  *  2.0; 

else  if  (freq  >  15)  freq  =  freq  *  (1,0  +  ((freq  -  15,0)/2.0)); 

} 

else 

{ 

/*  Sensor  Not  Counting  */ 
f  req  =  0.0; 

} 

/*Polyfit  for  Calibration  data  in  marco : /vaul t2/inarco/AUV/turbo_i)robe/tp .in  */ 

if (freq  >=  4999.0) 

{ 

return {fabs( speed) ) ; 

} 

else 

{ 

return (0 . 00000973701619*freq*f req  +  0 . 02934498907499*f req  + 
0.15845400316984)  ; 

} 

} 

/******************************★****★***************************************/ 

/*  — -  VERIFIED  MATCH  CURRENT /OPERATIONAL  MARCO  AUV  HARDWARE /SOFTWARE  — -  */ 

void  zero_gyro_data  () 

{ 

int  index,  val; 

int  save_trace  =  TRACE;  /*  save  current  TRACE  value,  restore  later  */ 
if  (TRACE  &&  DISPLAYSCREEN)  printf  ("[start  zero_gyro_data  ()]\n"); 

/*  Marco  code  has  a  mode  variable  for  gyro  on/off,  we  assume  always  on  */ 
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if  (fabs  (dg_offset)  <  0.001)  dg_offset  =  0.0; 
/*z_val0  =  adcl{DEPTH_CELL_CH) ; 

yaw_rate_0  =  get_adcl  (YAW_RATE_CH)  ;  */ 


z^valO  =  get_adc2  (DEPTH_CELL_CH,  0)  ; 

pitch_0  =  get_adc2  {PITCH_ANGLE_CH.  0)  ; 

roll_0  =  get_adc2 (ROLL^ANGLE^CH, 0) ; 

dg_offset  =  read_heading  (); 
roll_rate_0  =  get_adc2  (ROLL_RATE_CH,  0 )  ; 
pitch_rate_0  =  get_adc2  (PITCH_RATE_CH,  0)  ; 
yaw_rate__0  =  get_adc2  {YAW_RATE_CH,  0)  ; 


for  (i=0;i<9;++i) 

{ 

/*z_val0  +=  add (DEPTH_CELL_CH) ; 

yaw_rate_0  +=  get_adcl {YAW_RATE_CH) ; */ 

get_adc2 (11,0); 
get_adc2 (12,0) ; 
get_adc2 (9,0); 
gGt_adc2 (8,0); 
get_adc2  (YAW_RATE_CH,  0 )  ; 
read_heading ( ) ; 
get_adc2 (DEPTH_CELL_CH, 0 ) ; 

tsleep (5 ) ; 

} 


pitch  0 

+= 

roll^O 

+= 

roll_rate__0 

+= 

pitch_rate_0 

+= 

yaw_rate_0 

+= 

dg__of  f  set 

+= 

z_val0 

+= 

dg_of f set 

z_val0 

pitch_0 

roll_0 

roll_rate_0 

pitch_rate_0 


dg_of f set/10 .0; 

z_val0/10; 

pitch_0/10; 

roll_0/10; 

roll_rate_0/10; 

pitch_rate_0/10  ; 


yaw_rate_0  =  yaw_rate_0/10; 


/*psi_bit_old  =  Read_PortAB(MFI_BASE)  ;*/ 
psi_bit_old  =  Read_PortAB(0xFFF00700)  ; 
psi_bit_old  &=  0x3 FFF; 


if  (TRACE  ScSc  DISPLAYSCREEN) 

{ 

printf . {"roll_0  =  %d\n", 

printf  ("roll_rate_0  =  %d\n", 
printf  ("pitch_0  =  %d\n", 

printf  ("pitch_rate_0  =  %d\n" , 
printf  ("yaw_rate_0  =  %d\n'' , 
printf  ("z^valO  ==  %d\n", 

printf  ("dg_offset  =  %f\n", 

} 


roll_0) ; 
roll_rate_0)  ; 
pitch_0) ; 
pitch_rate_0) ; 
yaw_rate_0 ) ; 
z_valO)  ; 
dg_offset) ; 


if  (TRACE  ScSc  DISPLAYSCREEN)  printf  ("[finish  zero_gyro_data  ()]\n"); 
return; 


}  /*  end  zero  gyro  data  ()  */ 

/***************★***********★***********************************************/ 

/*  -  VERIFIED  MATCH  CURRENT /OPERATIONAL  MARCO  AUV  HARDWARE /SOFTWARE  ---  */ 

void  zero^surf aces  ()  /*  Initialize  all  planes  &  rudders  to  zero  */ 

{ 

if  (TRACE  ScSc  DISPLAYSCREEN) 

printf  ("[start  initializG_dacs_zero_actuators  () ] \n" ) ; 
command_rudder  (0.0) ; 
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coiranand_pl anes  (0.0)  ; 

if  (TRACE  ScSc  DISPLAYSCREEN) 

printf  ("[finish  zero_surf aces  ()]\n"); 

return; 


}  /*  end  zero_surfaces  ()  */ 

/**********★**************************************************************** ^ 

/*  -  VERIFIED  MATCH  CURRENT /OPERATIONAL  MARCO  AUV  HARDWARE /SOFTWARE  -  ^/ 

void  initialize_adcs  (}  /*  reconsider  casts  <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<  */ 
{  /*  is  this  code  even  needed  <<<<<<<<<<<<<<<<<<<<<<<  */ 

int  i,j,val; 


if  (LOCATIONLAB) 

{ 

return; 

} 

if  (TRACE  &&  DISPLAYSCREEN)  printf  ("[start  ini tialize_adcs  ()]\n"); 

/*  Init_PortA(MFI_BASE,0)  ; 

Init^PortB (MFI^BASE, 0) ;  */ 


Init_PortA(0xFFF00700, 0) ;  /*  appears  to  be  archaic  «««««  */ 

Init_PortB (0XFFF00700, 0) ; 

for  (i=0;i<4;++i)  /*  Stabilize  ada-1  card  by  reading  it  a  few  times  */ 

for(j=0; j<8;++j ) 

{ 

val  =  get_adcl  (j); 

} 

} 

if  (TRACE  ScSc  DISPLAYSCREEN)  printf  ("[finish  ini tializ evades  ()]\n"); 
return; 


)  /*  end  initialize_adcs  */ 

/*********★************************************************★****************/ 
void  init_pia  () 

{ 

if  (LOCATIONLAB) 

{ 

return; 

} 

viaO [ORA_IRA]  =  OxFF; 
viaO[ORB_IRB]  =  OxFF; 

viaO [DDRA]  =  OxFF;  /*  Enable  VIAO  for  Writing  */ 
viaO [DDRB]  =  OxFF; 

ViaO [ORA^IRA]  =  OxFF; 
viaO[ORB_IRB]  =  OxFF; 

viaOa_reg  =  OxFF; 
viaOb_reg  =  OxFF; 

vial [DDRA]  =  0x00;  /*  Enable  VIAl  for  reading  */ 
vial [DDRB]  =  0x00; 

tsleep(lOO);  /*  Let  Things  Cool  Off  */ 

} 
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/*  Initialize  tim_la  cards,  mode  =  0  init  encoders  only,  init  =  1  encoders  */ 
/*  and  fins  */ 


void  ini t_timla (mode) 
int  mode; 

{ 

int  i,j; 


/* 


*/ 


if  (LOCATIONLAB) 
{ 

return; 

} 


counter  1, 

Card 

counter  2, 

Card 

counter  3, 

Card 

counter  4, 

Card 

if (mode) 

{ 


front  rudder  top,  rear  rudder  bottom 
front  rudder  bottom,  rear  rudder  top 
front  plane  left,  rear  plane  right 
front  plane  right,  rear  plane  left 


/*  Init  control  surface  card  1  */ 

write_timla(l, tim_la_control_reg,255) ;  /*  reset  all  board  functions  */ 
wri te_timla ( 1 , tim__la_control_reg, 23 ) ;  /*  select  mastermode  register  */ 

wri te_timla  { 1 ,  t im_la__data_reg,  17 6 ) ;  /*  lobyte  enables  8  bit,  binary,  */ 

/*  fout  */ 

write_timla  (1,  tim_la__data_reg,  65)  ;  /*  hibyte  enable  fout  =  Imhz  etc  */ 

write_timla (1, tira_la__control_reg, 249) ;  /*  disable  write  prefetch  */ 


/*  for  (i=25; i<=28; i++)  Use  this  if  new  chip  installed  */ 

for  ( j=9; j<=12 ; j++)  /*  This  is  done  since  signal  gets  inverted  */ 

/*  Counters  1-4  Only  */ 

{ 

write_timla(l, tim_la_control_reg, j);  /*  high  output  time  about  Sms  */ 
write_timla(l, tim_la_data_r€g,0) ;  /*  load  all  hold  registers  */ 

write_timla  (1,  tim_la_data_reg,  150) ;  /*  lobyte  =  0  hibyte  =  155  for  */ 


for  (j=l; j<=4; j++)  /*  Counters  1-4  Only  */ 

{ 

write_timla(l, tim_la_control__reg, j);  /*  program  all  counter  mode  */ 

/*  registers  see  mode  j  */ 

write_timla{l,  tim_la__data_reg,98) ;  /*  lobyte  =  reload  from  load  */ 

/*  or  hold,  count  repeat  */ 

write_timla (1, tim_la_data_reg, 27 ) ;  /*  higyte  =  nogate, count  on  .*/ 

/*  falling  edge  Imhz  */ 

}  /*  End  if (mode)  */ 

/*  Init  speed  Sensor  cards  2  &  3  */ 


counter 

1, 

Card 

counter 

2, 

Card 

counter 

3, 

Card 

counter 

4, 

Card 

counter 

1, 

Card 

counter 

2, 

Card 

counter 

3, 

Card 

BOW  VERTICAL  THRUSTER  SPEED 
BOW  LATERAL  THRUSTER  SPEED 
STERN  VERTICAL  THRUSTER  SPEED 
STERN  LATERAL  THRUSTER  SPEED 
LEFT  SCREW  SPEED 
RIGHT  SCREW  SPEED 
TURBO  PROBE  SPEED 
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/ 


for {i=2; i<=3 ;++i)  /*  Program  Master  Mode  Reg.  for  Cards  2  &  3  */ 

{ 

write_timla(i, tim_la_control_reg,Oxff) ;  /*  Reset  All  Board  Functions  */ 
write_timla (i,tim_la_control_reg,0xl7) ;  /*  Select  Master  Mode  Reg,  */ 
write^timla  (i,  tim_la_data_reg,  OxbO )  ; 
write__timla  (i ,  tim_la_data_reg,  Oxcl )  ; 

} 

for ( j=l; j<=4;++j )  /*  Program  Counters  1-4,  Card  2  */ 

{ 

wri  te_timla  (2 ,  tim_la_control_reg,  j  )  ; 
write_timla  (2,  tim_la_data_reg,Oxaa)  ; 

write_timla (2, tim_la_data_reg,203) ;  /*  Set  for  FI  (IMhz)  */ 


for( j=9; j<=12;++j)  /*  Set  LOAD  Reg  1-4  to  Zero,  Card  2  */ 

{ 

write_timla (2 , tim_la_control_reg,  j ) ; 
write_timla  (2 ,  tim„la_data_reg,  0x00)  ; 
wri te_timla  (2 ,  tim_la_data_reg,  0x00)  ; 

) 

write_timla(2, tim_la_control_reg,0x4f ) ;  /*  Load  Counters  1-4  Card  2  */ 
write_timla(2, tim_la_control_reg,0x2f ) ;  /*  Arm  Counters  1-4  Card  2  */ 

write_timla(2,tim_la_aux_gates_reg,0xff )  ;  /*  SET  AUX  GATES  HIGH  TO  WORK!  */ 

for(  j  =  l;  j<=:3;++j)'  /*  Program  Counters  1-3,  Card  3  */ 

{ 

write_timla  (3  ,  tim_la_control_reg,  j  )  ; 
write__timla  (3  ,  tim^la_data_reg,  Oxaa)  ; 


/*  FI  =  203 

=  OxCB  = 

1 

Mhz 

F2  =  204 

=  OxCC  = 

100 

Khz 

F3  =:  205 

=  OxCD  = 

10 

Khz 

F4  =  206 

=  OxCE  = 

1 

Kz 

F5  =  207 

=  OxCF  = 

100 

Hz 

*/ 


if { j==3) 

{ 

/*  Turbo  Probe  */ 

write_timla (3, tim_la_data_reg, 205) ;  /*  Set  Counter  3  for  F  (hz)  */ 

} 

else 

{ 

write_timla(3,  tim_la_data_reg,2  03) ;  /*Set  Counters  1-2  for  FI  (IMhz)*/ 


for{ j=9; j<=:ll;++j)  /*  Set  LOAD  Reg  1-3  to  Zero,  Card  3  */ 

{ 

write_timla  (3 ,  tim_la__control_reg,  j )  ; 
write_timla  (3 ,  tim_la__data_reg,  0x00)  ; 
write_timla (3 ,  tim_la_data_reg, 0x00) ; 

} 

write_timla{3, tim_la_control_reg,0x47) ;  /*  Load  Counters  1-3  Card  3  */ 
write_timla (3, tim_la_control_reg,0x27) ;  /*  Arm  Counters  1-3  Card  3  */ 

writG__timla (3, tim_la_aux_gates_reg, Oxff ) ; /*  SET  AUX  GATES  HIGH  TO  WORK!  */ 


void  thruster_^ower (onoff ) 
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/*  A  signal  inverter  has  been  placed  between  the  pia  card  and  the  power 
supplies  for  the  thrusters,  so  in  order  to  turn  them  on,  bits  for  these 
must  be  set  low  */ 

int  onoff; 

{ 

if  (LOCATIONLAB) 

{ 

return; 

} 

switch (onoff) 

{ 

case  0:  /*  TURN  OFF  */ 

via0a_reg  =  viaOa_reg  I  0x3C;  /*  Set  bits  PA2-PA5  High  retaining  */ 

/*  other  bits  */ 

viaO [ORA_IRA]  =  viaOa_reg; 
break; 

case  1: 

via0a_reg  =  via0a_reg  &  0xC3 ;  /*  Set  bits  PA2-PA5  Low  retaining  */ 

/**  other  bits  */ 

viaO  I0FUV_IRA]  =  viaOa_rGg; 
break; 

} 

} 

void  screw__power (onoff ) 

/*  A  signal  inverter  has  been  placed  between  the  pia  card  and  the  power 
supplies  for  the  thrusters,  so  in  order  to  turn  them  on,  bits  for  these 
must  be  set  low  */ 

int  onoff; 

{ 

if  (LOCATIONLAB) 

{ 

return; 

} 

switch (onoff) 

{ 

case  0:  /*  TURN  OFF  */ 

via0a_reg  =  via0a_reg  I  0x03;  /*  Set  bits  PA0~PA1  High  retaining  */ 

/*  other  bits  */ 

viaO [ORA_IRA]  =  via0a_reg; 
break; 

case  1: 

viaOa_reg  =  via0a_reg  &  OxFC;  /*  Set  bits  PAO-PAl  Low  retaining  */ 

/*  other  bits  */ 

viaO [ORA_IRA]  =  via0a_reg; 
break; 

} 

} 

/★************************************/ 

/****** ************************** ****★/ 
/**********★********★*********★********/ 


/*  -  VERIFIED  MATCH  CURRENT/ OPERATIONAL  MARCO  AUV  HARDWARE /SOFTWARE  -  */ 

void  coinmand_control_surface  (angle,  surface) 


double  angle; 
int  surface; 

This  function  sends  the  desired  ANGLE  to  the  specified  control  SURFACE 
The  angle  is  first  normalized  to  (-45  to  45  degrees),  then  correction  is 
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applied  for  the  nonlinearity  in  the  servo  control  module 

/*  Connections  are: 

pin  1  =  control 

pin  2  =  ground 

pin  3=5  volts 

useful  pulse  widths  are  600  to  2500  ms 
this  program  ouputs  positive  going  pulses  with  a  8  ms  delay  between  pulses 
this  program  is  set  up  for  a  1  MHz  board 

int  skip_pulse; 

int  pulse, hipulse, lopulse, n, m, dis,  larm; 
unsigned  char  card; 

int  volt;  /*  archaic  */ 

double  a,b,c,d;  /*  archaic  */ 

int  old_pulsel  =  -1;  /*  Init  Old  Pulses  for  cont.  surf race  servos  */ 

int  old_pulse2  =  -1; 

int  old_pulse3  =  -1; 

int  old__pulse4  =  -1; 

if  (FALSE  &5c  DISPLAYSCREEN) 

printf  ( " [ start  command_control_surface  ( ) ] \n" ) ; 

if  (LOCATIONLAB) 

{ 

return;  /*  no  action  required  in  virtual  world 

} 

/*  pulse  =  39.32*angle  +  5171;*/  /*  angle  (deg)*/ 

pulse  =  ((int)  2252 . 87*angle)  +  5171;  /*  Calib  for  Vehicle  Servo  angle 

(rad)  */ 

hipulse  =  pulse/256; 

lopulse  =  pulse  -  (hipulse*256 ) ; 

skip_pulse  =  FALSE; 

switch (surface) 

{ 

case  1 : 
n  =  25; 
m  =  233; 
dis  =  OxCl; 
larm  =  0x61; 

if (pulse  ==  old_pulsel)  skip_pulse  =  TRUE; 

old_pulsel  =  pulse; 

break; 

case  2 : 
n  =  26; 
m  =  234; 
dis  =  0xC2; 
larm  =  0x62; 

if (pulse  ==  old_pulse2)  skip_pulse  =  TRUE; 

old_pulse2  =  pulse; 

break; 

case  3 : 
n  =  27; 
m  =  2  3  5  ; 
dis  =  0xC4; 
larm  =  0x64; 

if (pulse  ==  old_pulse3)  skip_pulse  =  TRUE; 
old_pulse3  =  pulse; 


break; 


case  4 : 
n  =  28; 
m  =  236; 
dis  =  0xC8; 
larm  =  0x68; 

if  {pulse  ==  old__pulse4)  skip_pulse  =  TRUE; 

old_jpulse4  =  pulse; 

break; 

default : 

if  (DISPLAYSCREEN)  printf (" Invalid  surface  code\n")/ 
break; 

} 

if ( !skip_pulse)  /*  SKIP  resetting  of  freq  out  if  command  angle  has  not*/ 
/*  changed.  Otherwise  servo  will  chatter  at  frequency  */ 
/*  of  control  loop  when  command  angle  does  not  change  */ 

write_timla  (1 ,  tim_la_control_reg,  dis)  ; 
write_timla  (1 ,  tim_la_control_reg,  n)  ; 
write_timla  (1 ,  tim_la_data_reg,  lopulse) ; 
write_timla  (1,  tim_la_data_reg,hipulse)  ; 
write_timla  (1 ,  tim_la_control_reg,m)  ; 
write_timla  (1 ,  tim_la_control__reg,  larm) ; 

} 

if  (FALSE  ScSc  DISPLAYSCREEN) 

printf  (" [finish  command_control_surf ace  ()]\n"); 

return; 

}  /*  command_control_surface  ()  */  ■ 


/********************************************************★*********★********/ 

/* VERIFIED  MATCH  CURRENT /OPERATIONAL  MARCO  AUV  HARDWARE /SOFTWARE - */ 

void  command_rudder  (angle) 

/*  Send  angular  deflection  (DEGREES)  to  rudders. 

Convention  (+)  angle  forward  rudder  =>  auv  right  turn, 

(-)  angle  forward  rudder  =>  auv  left  turn  */ 

double  angle; 

{ 

if  (TRACE  &Sc  DISPLAYSCREEN)  printf  ("[start  commander udder  {)]\n"); 

/*top/bottom  surfaces  are  slaved  due  to  inadequate  DAC  card  channels  */ 

/*positive  forward  rudder  angle  pushes  bow  to  right  =>  positive  psi  rate*/ 

angle  =  radians  (angle);  /*  convert  degrees  to  radians  */ 

command_control_surface  (  angle,  BOW__RUDDER_TOP  )  ; 

/*  command_control_surface  (  angle,  BOW_RUDDER_BOTTOM  );  hardware  error  */ 
coinmand_control_surface  (-angle,  STERN_RUDDER_TOP  ); 

.  /*  command_control_surface  (-angle,  STERN_RUDDER_BOTTOM) ;  hardware  error  */ 

if  (TRACE  ScSc  DISPLAYSCREEN)  printf  ("[finish  coiriimand_rudder  ()]\n"); 

return; 

}  /*  end  command_rudder  ()  */ 

/****************************** ********************************^^^**^^***^*^^ 
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/*  -  VERIFIED  MATCH  CURRENT/ OPERATIONAL  MARCO  AUV  HARDWARE/ SOFTWARE  -  */ 

void  command_planGs  (angle) 

/*  Send  angular  deflection  (RADIANS)  to  bow  and  stern  planes.  Convention: 
(-)  bow  plane  angle  =>  auv  dive,  (+)  bow  plane  angle  =>  auv  rise 
•  where  auv  dive  =  positive  depth  rate,  auv  rise  =  negative  depth  rate 
V 


double  angle; 

{ 

if  (TRACE  ScSc  DISPLAYSCREEN)  printf  ("[start  command_jp lanes  ()]\n"); 
/*  left/right  surfaces  are  slaved  due  to  inadequate  DAC  card  channels 

*/ 


/*  positive  planes  angle  pushes  bow  up,  yields  negative  depth  rate 

*/ 


angle  =  radians  (angle) ; 

command_control_surface  (  angle,  BOW_PLANE_STBD  ) ; 

/*  command_control_surface  (-angle,  BOW__PLANE_PORT  );coinbined  stern  stbd  */ 
coinmand_control_surface  (-angle,  STERN_PLANE_STBD)  ; 

/*  coinmand_control_surface  (  angle,  STERn1plane_P0RT)  ; combined  bow  stbd  */ 

if  (TRACE  ScSc  DISPLAYSCREEN)  printf  ("[finish  commandj lanes  ()]\n"); 

return; 

}  /*  end  command ^planes  ()  */ 

/********************★****************************************************** ^ 

/*  -  VERIFIED  MATCH  CURRENT/ OPERATIONAL  MARCO  AUV  HARDWARE /SOFTWARE  */ 

void  command_propellors_off  ()  . /*  Turn  off  both  main  propellors  */ 

if  (TRACE  &&  DISPLAYSCREEN) 

printf  ("[start  coinmand_propellors_of f  ()]\n"); 

command_mo  t or  (0.0,  PORT_PROP ) ; 
command_motor  (0.0,  STBD^PROP ) ; 

if  (TRACE  &&  DISPLAYSCREEN) 

printf  ("[finish  command_propellors_off  ()]\n"); 

return; 

}  /*  end  command_propellors_of f  ()  */ 

/*************************************************************************^*^ 

/*  ---  VERIFIED  MATCH  CURRENT/ OPERATIONAL  MARCO  AUV  HARDWARE /SOFTWARE  — -  */ 

void  command_thrusters_of f  ()  /*  Turn  off  both  main  propellers  */ 

if  (TRACE  ScSc  DISPLAYSCREEN) 

printf  ("[start  command_thrusters_of f  ()]\n"); 

coinmand_motor  (0.0,  BOW_VERTICAL)  ; 
command_motor  (0.0,  STERN_VERTICAL) ; 
command__motor  (0.0,  BOW_LATERAL)  ; 
coinmand__motor  (0.0,  STERN_LATERAL)  ; 

if  (TRACE  &&  DISPLAYSCREEN) 

printf  ("[finish  coTnmand_thrusters_of f  ()]\n"); 
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return; 


}  /*  end  coinmand_thrusters_of f  {)  */ 


/*  -  VERIFIED  MATCH  CURRENT/OPERATIONAL  MARCO  AUV  HARDWARE/SOFTWARE  --- 

void  command_motor  (order,  motor) 
double  order;  int  motor; 


motor  =  0 

Left 

Propeller 

PORT  PROP 

RPM 

1 

Right 

Propeller 

STBD  PROP 

RPM 

2 

Bow 

Vertical 

Thruster 

BOW  VERTICAL 

volts 
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Thruster 

STERN_VERTICAL 

volts 

4 
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5 
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Thruster 

STERN^LATERAL 

volts 

/*  use  local  variables  to  permit  clamping  without  side  effects 

int  dac_value  =  0;  /*  range  0..1023 

double  propellor_rpm  =  order;  /*  propellers  -700..  700  rpm 

double  thruster_volts  =  order;  /*  thrusters  -24..  24  volts 

if  (TRACE  &&  DISPLAYSCREEN) 

printf  ( " [ start  commandjnotor  ( ) ] Xn" ) ; 


if 

{ 


( (motor  ==  PORT_PROP)  I  I  (motor  ==  STBD_PR0P) ) 

clamp  (&propellor_rpm,  -700.0,  700.0, 

"command_motor  ()  :  propellor_rpm'’)  ; 


if  (motor  ==  PORT_PROP) 

dac_value  =  port_speed_control  (propellor_rpm  /  60.0); 
if  (motor  ==  STBD_PROP) 

dac_value  =  stbd^speed_control  (propellor_rpm  /  60.0); 

if  (TRACE  &&  DISPLAYSCREEN) 

if  (motor  ==  PORT_PROP)  printf  ("[PORT  "); 

else  if  (motor  ==  STBD_PROP)  printf  ("[STBD  "); 
printf  ("propellor_rpm  =  %5.1f,  dac_value  =  %d]\n", 
propel lor_rpm,  dac_value) ; 

} 

else  if  (  (motor  ==  BOW_VERTICAL)  I  I  (motor  ==  STERN_VERTICAL) 

^  I  I  (motor  ==  BOW_LATERAL)  I  I  (motor  ==  STERN_LATERAL  ) ) 

clamp  (Scthruster_volts,  -24.0,24 .0, 

"coramand_motors  ():  thruster_volts") ; 


dac_value  =  (int)  ( (thruster_volts  +  24.0)  *  1023.0  /  48.0); 

if  (TRACE  &&  DISPLAYSCREEN) 

printf  (" (thruster_volts  =  %5.1f,  dac_value  =  %d]\n", 

thruster_volts,  dac  value) ; 

}  -  !  > 

else  /*  erroneous  motor  number  selected  */ 

if  (TRACE  &&  DISPLAYSCREEN) 

printf  (" [command_motor  () :  erroneous  order/motor  (%5 .lf/%d) ] \n 

order, motor) ; 

return; 


send_dac2b  (dac_value,  motor) ; 
if  (TRACE  &&  DISPLAYSCREEN) 


printf  ("[finish  command_motor  ()]\n"); 
re turn ; 

}  /*  end  coniinand_motor  ()  */ 

/****** *************************************************************^*^^^^^^^ 

/* VERIFIED  MATCH  CURRENT/ OPERATIONAL  MARCO  AUV  HARDWARE /SOFTWARE */ 

void  test_alive  (interval,  local_start_dwell )  /*no  longer  used  by  Dave?  «<  */ 


unsigned  int  i interval, j interval ; 
double  test_delta; 

if  (TRACE  ScSc  DISPLAYSCREEN)  printf  ("[start  test_alive  {)]\n"); 

local_start_dwell  =  local_start_dwel 1*100 ; 
interval  =  interval*100; 
iinterval  =  local_start__dwell/interval  ; 
j interval  =0; 

test_delta  =  .4;  /*  Deflect  22.5  degrees  */ 

while (j interval  <  iinterval) 

{ 

command__control_surface  (BOW_RUDDER_TOP,  test_delta) ; 
tsleep { interval ) ;  /*  256ths  of  a  second  */ 

test_delta  =  -  test__delta; 
j interval  =  j interval  +  1; 

} 

tsleep  (200);  /*  256ths  of  a  second  */ 

if  (TRACE  &&  DISPLAYSCREEN)  printf  ("[finish  test_alive  ()]\n"); 

return; 

} 

/**ic*icic  ************-k*i,***ic*  **************  **ic-h**********icic  ********  ***********/ 
/*  -  NOT  YET  UPDATED  TO  CURRENT /OPERATIONAL  MARCO  AUV  HARDWARE/ SOFTWARE  --  */ 
void  get_init_avg  ()  /*  sonar  ???  better  name  needed  iil!  ««  */ 

int  index,  rng_sum; 

if  (TRACE  &Sc  DISPLAYSCREEN)  printf  ("[start  get__init_avg  ()]\n"); 

rng__sum  =  0; 
range_index  =  0; 

for (index  =  0;  index  <  AVG_PTS;  ++index) 

{ 

viaO [ORB_IRB]  =  (SONAR_SWl  &  SONAR_SW3)  [  SONAR_TRIG2 ; 
viaO[ORB_IRB]  =  SONAR_SWl  &  SONAR_SW3 ; 
tsleep (5) ; 

range  =  get_adc2  (3,0); 

rng_sum  +=  range; 
range_ar ray [index]  =  range; 

++range_index; 


} 

avg_rng  =  (rng_sum/AVG_PTS)  *  1.0; 
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if  (TRACE  ScSc  DISPLAYSCREEN)  printf  ("[finish  get_init_avg  ()]\n"); 
return; 

} 

/*****★********************************************** **********************^^ 

/*  --  NOT  YET  UPDATED  TO  CURRENT /OPERATIONAL  MARCO  AUV  HARDWARE/ SOFTWARE  --  */ 

void  get_avg_rng  () 

{ 

int  index,  UPDATE_AVG,  int_rng_suin; 

if  (TRACE  &&  DISPLAYSCREEN)  printf  ("[start  get_avg_rng  ()]\n"); 

UPDATE_AVG  =  0; 
int_rng_suni  =  0; 

if  (( (double) range  >  avg_rng  )  II 

(fabs( (double) range  avg_rng)  <=  MAX_RNG_DIFF)  II 
(bad_rng  >=  MAX_BAD_PTS) ) 

{ 

range_array [range_index]  =  range; 

++range_index; 

UPDATE_AVG  =  1; 
if(bad_rng  >  MAX_BAD_PTS) 

{ 

+ + b  ad_upda  t  e  s ; 

} 

if (bad_updates  >=  MIN_NO_PTS) 

{ 

bad__rng  =  0; 

} 

} 

else 

{ 

++bad_rng; 

} 

if (UPDATE^AVG) 

{ 

for (index  =  range_index  -  AVG_PTS;  index  <=  rangG_index;  ++index) 
int_rng_sum  +=  range_array  [index]  ; 

} 

avg_rng  =  int_rng_suin/AVG_PTS  *  1.0; 

} 

if  (TRACE  ScSc  DISPLAYSCREEN)  printf  ("[finish  get_avg_rng  ()]\n"); 
return; 

} 

/*************************************************************************** ^ 
/* 

Lab  hardware  control  changes  *FOLLOWING*  hardware  upgrade  1993 : 

Telemetry  to  tactical  level:  serial  port  /T1  via  driver  /TT 

Orders  from  tactical  level:  parallel  port  /P  via  MFI  register  A 

Sonar:  interface  card  device  driver  /T3 

*/ 

/***********************************************************^*^**^**^**^^^^^^ 

/*  -  NOT  YET  UPDATED  TO  CURRENT/OPERATIONAL  MARCO  AUV  HARDWARE /SOFTWARE  - 

*/ 
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void  open_device_paths  {) 

{ 

if  (TRACE  ScSc  DISPLAYSCREEN)  printf  ("[start  open_device_paths  ()]\n"); 

if  (LOCATIONLAB  ==  FALSE) 

{ 

/*  either  /tl  serial  port  #1  or  /tt  (high  baud  rate  driver  for  / 
tl)  */ 

serialpath  =  open  {"/tl",  S_IREAD  +  S_IWRITS)  ;  /*  get  path  number  */ 

/*  /tt  is  device  for  high  baud  rate  /tl  serial  port  */ 

if  (serialpath  <=  0) 

{ 

if  (DISPLAYSCREEN) 

{ 

printf  ("open_device__paths  ()  :  unable  to  open  serialpath  /tl.  "); 
printf  ("Exit. \n") ; 

) 

exit  (-1); 

} 

if  (TRACE  ScSc  DISPLAYSCREEN) 

printf  ("[serialpath  /tl  (normal  baud  rate)  open,  path  number  =  %d]\n", 

serialpath) ; 

if  (SONAR INSTALLED) 

{ 

sonarpath  =  open  ("/t3",  SPREAD  +  S_IWRITE)  ;  /*  get  path  number  */ 

/*  /t3  is  device  for  sonar  interface  card  */ 

if  (sonarpath  <=  0) 

{ 

if  (DISPLAYSCREEN) 

{ 

printf  ("open_device_paths  ():  unable  to  open  sonarpath  /t3.  "); 

printf  ("Exit.Xn"); 

} 

exit  (-1); 

} 

if  (TRACE  &&  DISPLAYSCREEN) 

printf  ("[sonarpath  /t3  open,  path  number  =  %d]\n",  sonarpath); 
tty_mode  (sonarpath,!);  /*  initialize  sonar  values  */ 

} 

else  if  (TRACE  &&  DISPLAYSCREEN) 

printf  {"[sonarpath  /t3  ignored,  SONARINSTALLED  ==  FALSE] \n" ) ; 

/*  other  paths:  effectors,  depth__sonar,  etc.  ************************/ 

} 

if  (TRACE  ScSc  DISPLAYSCREEN)  printf  ("[finish  open_device_jpaths  ()]\n"); 
return; 

} 

/************************ ***************************************************^ 

void  tty_mode  (path,  mode) 
int  path,  mode; 

{ 

static  struct  sgbuf  old, new; 
static  int  init  =  1; 
int  status; 

if (init) 

{ 

init  =  0; 

status  =:  _gs_opt  (path,&old)  ; 
status  =  _gs_opt  (path,  anew); 
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new.sg_class  =  0; 
new .  sg__case  =  0  ; 
new.  sg__backsp  =  0; 
new. sg_delete  =  0; 
new.sg_echo  =  0; 
new.sg__alf  =  0; 
new.sg_nulls  =  0; 
new.sg_pause  =  0; 
new .  sg__page  =  0  ; 
new.sg_bspch  =  0; 
new.sg_dlnch  =  0; 
new.sg_eorch  =  0; 
new.sg_eofch  =  0; 
new.sg_rlnch  =  0; 
new.sg_dulnch  =  0; 
new.sg_psch  =  0; 
new.sg_kbich  =  0; 
new.sg_kbach  =  0; 
new.sg_bsech  =  0; 
new. sg_bellch  =  0; 
new.sg_parity  =  0; 
new.gg_tabcr  =  0; 
new.sg_tabsiz  =  0; 
new.sg_tbl  =  0; 
new.sg_col  =  0; 
new.sg_err  =  0; 

} 

if (mode) 

__ss_opt  (path,&n€w)  ; 
else 

--SS_opt  (path.  Scold)  ; 
return; 


/*  --  NOT  YET  UPDATED  TO  CURRENT/ OPERATIONAL  MARCO  AUV  HARDWARE /SOFTWARE  */ 

void  close__device_paths  () 

{ 

if  (TRACE, ScSc  DISPLAYSCREEN)  printf  ("[start  close_device_paths  ()]\n"); 

if  (serialpath  >  0)  close  (serialpath)  ;  /*  test  for  open  before  closing  */ 

else  if  (TRACE  &&  DISPLAYSCREEN)  printf  ("[serialpath  was  not  openIjXn"); 

if  (SONARINSTALLED) 

{ 

if  (sonarpath  >  0)  close  (sonarpath); 

else  if  (TRACE  ScSc  DISPLAYSCREEN) 

printf {" [sonarpath  was  not  open!]\n"); 


/*  other  paths:  effectors,  depth_sonar,  etc.  <<<<<<<<<<<<<<<<<<<<<<<<<  */ 
if  (TRACE  ScSc  DISPLAYSCREEN)  printf  ("[finish  close_device_paths  ()]\n"); 
return; 


} 

/ 

/ 

/ 


*  NOT  YET  UPDATED  TO  CURRENT/OPERATIONALMARCO  AUV  HARDWARE /SOFTWARE  --  */ 

*  may  be  unneeded  in  next  version  */ 
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void  read_parallel_port  ()  /*  loop  and  display  8  bit  data  from  port  A  */ 

static  char  next_char,  last__char; 
static  char  current__command  [256]; 
static  int  index; 
unsigned  char  tempo¬ 
re  turn; 

if  (TRACE  ScSc  DISPLAYSCREEN) 

{ 

printf  ( " [ start  read_parallel_port  ( ) ,  " ) ; 

if  (PARALLELPORTTRACE)  printf  ( "PARALLEL PORTTRACE  is  ON]\n"); 
else  printf  ("PARALLELPORTTRACE  is  OFF]\n"); 

} 


/*  see  initializ€_adcs  ()  for  Init_PortA  &  B  code  */ 
#ifdef  os9 

/*  Read  PortA  parallel  port  character  by  character  for  tactical  orders  */ 
/*  reference:  Walt  Landaker's  mfi_a3.c  in  directory  /hO/AUV  and  */ 
/*  page  3-12  of  Motorola  6800  Series  Manual  for  6821  PIA  */ 
/*  Programmable  Interface  Adapter.  */ 

/*  Warning!  You  may  have  to  reset  both  computers  to  get  the  parallel  */ 
/*  port  to  read  &  write  properly.  Additionally,  */ 
/*  on  the  386  you  can  run  PORTFIX  to  reset  parallel  port  LPTl :  V 


temp  =  Read_PortA  ((struct  MFI_PIA  *)  MFIJASE)  ;  /*  should  clear  busy!  */ 

index  =  0; 

/*  read  port  status  (note  sta  not  stb)  */ 

•PortAFlag  =  ck_sta  ((struct  MFI_PIA  *)  MFI_BASE) ; 

if  (PARALLELPORTTRACE  &&  DISPLAYSCREEN) 

printf  ("\n  [time  %5.2f  read_parallel_^ort  ()  resumed]",  t) ; 


while  (PortAFlag  &&  0x80)  /*  see  loop  break  for  alternate  exit  */ 

{ 

/*  Note  that  ck_stb  is  used  in  mfi_a3  but  ck_sta  makes  more  sense  */ 
PortAFlag  =  ck_sta  ((struct  MFI_PIA  *)  MFI_BASE) ;  /*read  port  status  */ 
last_char  =  next_char;  /*  read  char  and  reset  busy  */ 

next_char  =  Read_PortA(  (struct  MFI_PIA  *)  MFI_BASE)  ; 

if  ((PortAFlag  ==  0x24)  &&  (last_char  ==  next_char) )  break; 

/*  if  next_char  changed  then  flag  may  be  messed  up,  read  anyway  */ 

/*  check  for  ptr  strobe  */ 

/*  break  =>  no  character  waiting  */ 

/*  control  passes  outside  while  loop  */ 

else  if  {next_char  ==  13)  /*  CR  indicates  end  of  line  */ 

{ 

current^command  [index]  =  13;  /*  CR  /n  */ 

current_command  [index+1]  =  10;  /*  LF  extra,  not  needed  */ 

current_command  [index+2j  =  0;  /*  end  of  string  delimiter  */ 

index  =  0; 


if  (auvtextf ile! =NULL) 

{ 

fprintf  (auvtextfile,  "%s",  current^command) ; 

fflush  (auvtextfile);  /*  force  completion  of  file  write  */ 

} 

if  (DISPLAYSCREEN) 

{ 

printf  ("\n\n»>  time  %5.2f  tactical  message  «<\n",  t); 
printf  ("%s",  current_command) ; 
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printf  ("\n"); 

} 

/********  insert  command  processing  logic  here  ««««««««  */ 


}  /* 


} 

else  if  (next_char  !=  10)  /*  LF  ignored,  others  appended 

,  { 

current_command  [index]  =  next__char; 
index  ++; 

if  (PARALLELPORTTRACE  &&  DISPLAYSCREEN) 

{ 


} 


/*  print  character  to  screen  */ 

printf  ("\n  %c  =  %2x,  PortAFlag  after  reading  =  %4x] 
next__char,  next_char,  PortAFlag)  ; 


end  while  loop  to  read  characters  from  port 


*/ 


*/ 


if  (PARALLELPORTTRACE  &&  DISPLAYSCREEN) 
{ 


printf ( 


} 

#endif 


'  [%c  =  %2x,  PortAFlag  =  %2x,  exiting  read_parallel_port  ()] 

next_char/  next_char,  PortAFlag) ; 


if  (TRACE  &&  DISPLAYSCREEN)  printf  ("[finish  read_parallel_port  ()]\n"); 
return; 

}  /*  end  read_para Heliport  */ 

/*************************************★******★******************************/ 
/*  Card  arrangement  in  AUV  *PRIOR*  to  hardware  upgrade  1993: 
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This  file  contains  the  functions  which  address  the  A/D  D/A  cards 
directly  in  terms  of  voltages.  */ 


/*************************************************************************** ^ 

/*  -  VERIFIED  MATCH  CURRENT/ OPERATIONAL  MARCO  AUV  HARDWARE /SOFTWARE  -  */ 

/*  this  function  is  full  of  unreachable  statements,  ask  Dave  ««««««<  */ 
unsigned  char  read_timlacl  (card,  reg) 
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unsigned  card; 

unsigned  char  reg; 

{ 

unsigned  char  data; 

if  {TRACE  &&  DISPLAYSCREEN)  printf  ("[start  read^timlacl  {)]\n"); 

switch  (card) 

{ 

case  1 : 

return (tim_lacl [reg] )  ; 

data  =  tim__lacl  [3 ]  ;  /*  Wait  2uS,  one  access  to  address  (RTC)  */ 

break; 

case  2 : 

return (tim_lac2 [reg] )  ; 

data  =  tim_lac2 [3] ;  /*  Wait  2uS,  one  access  to  address  (RTC)  */ 

break; 

case  3: 

return  (tiin_lac3  [reg] )  ; 

data  =  tim_lac3  [3] ;  /*  Wait  2uS,  one  access  to  address  (RTC)  */ 

break; 

default ; 

if  (DISPLAYSCREEN) 

printf  (" [read_timlacl  ():  invalid  card  (%d)]\n",  card) ; 
return  (0); 


if  (TRACE  &&  DISPLAYSCREEN) 

printf  ("[finish  read_timlacl  ()  =  %d]\n",  data); 

return  (data) ;  /*  is  this  correct  ??  */ 

}  /*  end  read_timlacl  ()  */ 

/******************************************************^********************^ 

/*  ---  VERIFIED  MATCH  CURRENT /OPERATIONAL  MARCO  AUV  HARDWARE /SOFTWARE  */ 

void  write_timla  (card,  reg,  value) 

unsigned  card; 

unsigned  char  reg,  value; 


unsigned  char  data;  /*  Dummy  Data  for  delay  */ 

if  { LOCATIONLAB)  return;  /*  avoid  bus  error  writing  to  restricted  memory  */ 

if  (TRACE  &&  DISPLAYSCREEN)  printf  {"[start  write_timla  ()]\n"); 

switch  (card) 

{ 

case  1: 

tim_lacl [reg]  =  value; 

data  =  tim_lacl[3];  /*  Wait  2uS,  one  access  address  (RTC)  */ 

break; 

case  2: 

tim_lac2 [reg]  =  value; 

data  =  tim_lac2[3];  /*  Wait  2uS,  one  access  address  (RTC)  */ 

break; 

case  3: 

tim__lac3  [reg]  =  value; 

data  =  tim_lac3[3];  /*  Wait  2uS,  one  access  address  (RTC)  */ 
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break; 


default : 

if _  (DISPLAYSCREEN) 

printf  ( " [write_timla  ()  error;  illegal  card  value  (%d)]\n"/  card) ; 
break; 

} 

if  (TRACE  &&  DISPLAYSCREEN)  printf  {"(finish  write^tiinla  {)]\n"); 
return; 

}  /*  end  write_timla  ()  */ 

/****************************************** **************^*^**^***^^^^^^^^^ 
/**************-k**-k*ic**icic*icic****ifi(*-k*ic±**-kic*ic**ic*i,-kic-k*ic-k*****it*-k-k**ic*if***ic* 

*  send_dacl {s,ch)  --  writes  signal  's'  to  ada-1  dac  channel  'ch' 

*  (allowable  channels  0-3) 
**********************************★********************★******★**********/ 

/*  --  NOT  YET  UPDATED  TO  CURRENT /OPERATIONAL  MARCO  AUV  HARDWARE /SOFTWARE  --  */ 

/*  no  longer  used...  */ 

void  send__dacl  (s,ch) 
int  s,ch; 

{ 

if  (LOCATIONLAB)  /*  in  virtual  world,  do  not  read  any  slots  */ 

return; 

} 


/* 

i f  (NOT_YET_REIMPLEMENTED) 

{ 


ch  =  ch  «  2; 

/*  dacl_a[ch]  =  s  »  2; 

/*  dacl_a[ch  +  DAC_LSB_OFFSET] 

/*}*/ 

return; 

}  /*  send_dacl  */ 


/*  offset  for  G-96  addressing  */ 
/*  write  upper  8  bits  to  MSB 
s  «  6;  /*  write  lower  2  bits  B3,B2  */ 


/********icic**-k*-k*icic:k**i:ic*****icic**ic‘k****-k*icicic**ifk-k*icifi:iz-k*ic*ik**icic***-kici:*-k*i:-k* 

*  send_dac2b  (s,ch)  --  writes  signal  's'  to  dac2b  dac  channel  'ch' 

*  (allowable  channels  0-15) 

******************************************************^^*****^^*^*^^^^^^^^ 
/* -  VERIFIED  MATCH  CURRENT/ OPERATIONAL  MARCO  AUV  HARDWARE /SOFTWARE - */ 


/*************************************************^^**^^^ 

*  dac2b(s,ch)  --  writes  signal  's'  to  dac2b  dac  channel 

*  (allowable  channels  0-7) 

*  s  =  0  — >  -  10  Vdc  Output 

*  3=5 12  — >  0  Vdc  Output 

*  s  =  1024  — >  +  10  Vdc  Output 


*  C 

*  I 


* 

R 

20* 

* 

C 

18* 

* 

Channel 

7 

--> 

16* 

* 

B 

Channel 

6 

--> 

14* 

* 

R 

Channel 

5 

--> 

12* 

* 

D 

Channel 

4 

--> 

10* 

* 

Channel 

3 

--> 

8* 

* 

S 

Channel 

2 

--> 

6* 

*19 

*17 

*15  <--  Ground 
*13  <--  Ground 
*11  Ground 
*9  <--  Ground 

*7  <--  Ground 

*5  <--  Ground 


'ch' 
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*  I 

Channel 

1  --> 

4* 

*3 

< —  Ground 

*  D 

*  E 

Channel 

0  --> 

2* 

*2 

<--  Ground  (Bad) 

* 


void  send_dac2b  {s,ch) 
int  s,ch; 


{ 

if  (LOCATIONLAB)  return;  /*avoid  bus  error  writing  to  restricted  memory 


*/ 


ch  =  ch  «  2;  /*  offset  for  G-96  addressing  */ 
dac2b_a[ch]  =  s  >>  2 ;  /*  write  upper  8  bits  to  MSB  */ 
/*dac2b_a[ch  +  DAC_LSB_OFFSET]  =  s  «  6;*/  /*write  lower  2  bits  B3,B2  */ 
dac2b_a[ch  +2]  =  s  «  6; 


return; 

}  /*  send__dac2b  */ 


'*****************************■*****•******************★★******************** 

*  get_adcl(n)  --  reads  ada-1  adc  channel  'n'  (channels  0-15) 

***************'5t*****ilf**:lt*****iit*************’****’**************************  j 


/*  VERIFIED  MATCH  CURRENT/ OPERATIONAL  MARCO  AUV  HARDWARE /SOFTWARE  */ 


int  get_adcl  (n) 
int  n; 

{ 

int  val; 

if  (LOCATIONLAB)  /*  in  virtual  world,  do  not  read  any  slots  */ 

{ 

return  ( 0 ) ; 

} 


/*adcl_a[ADCl_CMD_REG]  =  n; 
while  (adcl_a[ADCl_STATUS_REG]  >20); 
val  =  adcl_a[ADCl_MSB]  «  2; 
val  +=  adcl_a[ADCl_LSB]  »  6;*/ 

adcl_a[4]  =  n; 
while  (adcl_a[4]  >  20); 
val  =  adcl_a[0]  «  2; 
val  +=  adcl_a[2]  »  6; 


return  (val)  ; 


/ 

/ 


*  get_adcl  */ 

**************************************************************************** 


*  get__adc2  (n,g)  ;  --  Reads  adc-2  channel  'n'  (0-15) 

*  with  gain  'g'  (0  to  F  =>  0  -  1024) 


/*  ---  VERIFIED  MATCH  CURRENT/OPERATIONAL  MARCO  AUV  HARDWARE /SOFTWARE  ---  */ 

int  get_adc2  (n,g) 
int  n,g; 

{ 

int  val; 

if  (LOCATIONLAB)  /*  in  virtual  world,  do  not  read  any  slots  */ 

{ 

return  (0) ; 

} 

/*adc2_a  [ADC2„CH_GAIN]  =  (n  «  4)  I  g;*/  /*  set  c&g,  start  conv  */ 


204 


/*while( (adc2_a[ADC2_STATUS_REG]  &  0x7)  !=  0);*/  /*  wait  for  ready  */ 

adc2_a[0]  =  (n  «  4)  I  g;  /*  set  c&g,  start  conv  */ 

while  (  {adc2__a  [2]  &  0x7)  !=  0);  /*  wait  for  ready  */ 


/*  This  adc  uses  0  -  4095  to  represent  full  scale  input,  in  order 
to  write  to  the  dac  {which  uses  0  -1023  for  full  scale)  you 
must  divide  val  by  4  or  shift  right  by  2.  Use  the  next  line  to 
get  full  resolution, 
val  =  adc2_a [ADC2_DATA] ; 

The  next  line  is  used  for  testing  purposes  only 

val  =  adc2_a[ADC2_DATA]  »  2;  */ 

/*val  =  adc2_a [ADC2_DATA] ;*/ 

val  =  adc2_a[l]; 
val  =  val  &  OxOFFF; 

return  (val) ; 

}  /*  get_adc2  */ 

/**********************  *-k  ********  ic-k**ic*-kicic****ic*i,*-k**-k****ic*****icic-k*ic**  *****/ 
/* 

The  program  code  for  the  Multi-Function  Interface  originated  from  'mfi.c' 
Routines  include  Init^PortA,  Init^PortB,  Read^PortA,  Read_PortB,  Read_PortAB 

Excerpt  of  'mfi.c'  comments  follows: 

Program  example  for  the  Multi-Function-Interface  (MFI) 

This  example  uses  the  6821  PIA  on  the  MFI  board 

General  purpose  functions  are  provided  to  initialize  the  PIA 

and  read/write  data  to  the  ports 

MFI  P2  connector  definitions  are  provided  by  the  GESMFI-1  data 
sheet  available  from  GESPAC,  Inc. 

6821  device  specifics  are  covered  in  the  8-bit  microprocessor 
&  peripheral  data  book  from  Motorola  Inc. 

3/1/91  J.  Rawlins  RealTime  Software  Consulting 

******************************i,ici,ic*****************************ic*****ic******^ 


/****************************i(*ifi,ic**********************ic*ic**************** 

*  Ini t_PortA( base,  dir)  —  Initialize  Port  A  of  MFI 

*  dir:  1  =  output  port,  0  =  input  port 
**********************************************************************★**/ 

/* -  VERIFIED  MATCH  CURRENT /OPERATIONAL  MARCO  AUV  HARDWARE /SOFTWARE - */ 

void  Init__PortA  (base,  dir) 

register  struct  MFI^PIA  *base;/*  base  address  of  MFI  board  on  G96  bus 

*/ 

^  int  dir;  /*  direction:  1  =  output  port,  0  =  input  port  */ 

register  short  temp; 

if  (LOCATIONLAB)  /*  in  virtual  world,  do  not  read  any  slots  */ 

return; 

} 

temp  =  (base->cra  &  OxOOFF) ;  /*  get  current  value  of  control  A  */ 

temp  Sc-  ~4 ;  /*  clear  bit  #2  so  we  can  access  ddra  */ 

base->cra  =  temp; 

if  (  dir  )  /*  make  port  A  all  outputs  */ 
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else 


base->pra  =  OxOOFF; 


/*  port  A  is  all  inputs  */ 

base->pra  =  0x0000; 
temp  1=  4;  /*  set  bit  #2  to  access  data  registers  */ 

base->cra  =  temp; 

}/*  Init_PortA  */ 


*  Init_PortB  (base,  dir)  ---  Initialize  Port  B  of  MFI 

*  dir:  1  =  output  port,  0  =  input  port 
************************************************************************** ^ 


/*  ---  VERIFIED  MATCH  CURRENT /OPERATIONAL  MARCO  AUV  HARDWARE /SOFTWARE  ---  */ 


void  Init_PortB  (base,  dir) 

register  struct  MFI_PIA  *base;  /*  base  address  of  MFI  board  on  G96  bus 
*/ 

int  dir;  /*  direction:  1  =  output  base,  0  =  input  base  */ 

{ 

register  short  temp; 


if  (LOCATIONLAB)  /*  in 

{ 

return; 

} 

temp  =  (base->crb  &  OxOOFF); 
temp  &=  ~4; 
base“>crb  =  temp; 
if  (  dir  ) 

base->prb  =  OxOOFF; 
else 

base~>prb  =  0x0000; 
temp  [=  4; 
base->crb  =  temp; 

}/*  Init^PortB  */ 


virtual  world,  do  not  read  any  slots  */ 

/*  get  current  value  of  control  A 
/*  clear  bit  #2  so  we  can  access  ddra 

/*  make  port  B  all  outputs 

/*  port  B  is  all  inputs 

/*  set  bit  #2  to  access  data  registers 


*/ 

*/ 

*/ 

*/ 

*/ 


*  Read_PortA  (base)  —  returns  8  bit  value  from  port  A 

* 


/*  --  NOT  YET  UPDATED  TO  CURRENT /OPERATIONAL  MARCO  AUV  HARDWARE /SOFTWARE  --  */ 
/*  not  found!  */ 


unsigned  char  Read__PortA  (base) 

register  struct  MFI_PIA  *bas€;  /*  base  address  of  MFI  */ 

{ 

register  unsigned  short  temp; 

temp  =  base“>pra;  /*  read  data  reg. should  reset  busy  */ 

return (temp  &  OxOOFF);  /*  return  data  to  calling  program  */ 

} 


/************************************************************************** 

*  Read_PortB  (base)  —  returns  8  bit  value  from  port  B 

* 

*************************************************************************/ 
/*  --  NOT  YET  UPDATED  TO  CURRENT/ OPERATIONAL  MARCO  AUV  HARDWARE / SOFTWARE  --  */ 
/*  not  found!  */ 
unsigned  char  Read_PortB  (base) 

register  struct  MFI_PIA  *base;  /*  base  address  of  MFI  */ 

{ 
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register  unsigned  short  temp; 

if  (LOCATIONLAB)  /*  in  virtual  world,  do  not  read  any  slots  */ 

{ 

return  (0); 

} 

temp  =  base->prb; 
return (temp  &  OxOOFF) ; 


/**************************************************************** **^******* 

*  Read_PortAB  (base)  —  return  a  16  bit  value  from  ports 

*  A  and  B  combined  then  mask  off 

*  the  15  th  and  16  th  bits. 

*  Note:  PIA  PA0-PA7  is  the  LSB  and  PB0-PB7  the  MSB 

/*  -  VERIFIED  MATCH  CURRENT/ OPERATIONAL  MARCO  AUV  HARDWARE /SOFTWARE  -  */ 

unsigned  short  Read_PortAB  (base) 

register  struct  MFI_PIA  *base;  /*  base  address  of  MFI  */ 

register  unsigned  short  hi, lo, temp; 
if  (LOCATIONLAB)  /*  in  virtual  world,  do  not  read  any  slots  */ 


return  (0) ; 

) 

lo  =  (base->pra  &  OxOOFF);  /*  get  least  significant  byte  from  A  ■*/ 

hi  =  (base->prb  &  OxOOFF);  /*  and  most  significant  byte  from  B  */ 

temp  =  {(hi  «  8)  +  lo)  ;  /*  shift  hi  into  upper  byte  of  word  */ 

^  return  (  temp  );  /*  return  data  */ 

/****★********************************************************************** ^ 
/*  --  NOT  YET  UPDATED  TO  CURRENT /OPERATIONAL  MARCO  AUV  HARDWARE/ SOFTWARE  */ 
/*  not  found!  */ 

void  set_bsyA (base)  /*  sets  CB2  high  (for  busy  to  sending  port)  */ 

register  struct  MFI_PIA  *base;  /*  base  address  of  MFI  */ 


register  short  temp; 

if  (LOCATIONLAB)  /*  in  virtual  world,  do  not  read  any  slots  */ 

return; 

} 

temp  =  (base->cra  &  OxFF)  ;  /*  save  era  values  */ 

base->cra  =  0x38;  /*  8  bit  1=‘  CR2  high  */ 

base->cra  =  temp;  /*  restore  era  values  */ 


/*  --  NOT  YET  UPDATED  TO  CURRENT/OPERATIONAL  MARCO  AUV  HARDWARE/ SOFTWARE  --  */ 
/*  not  found!  */ 

/*  sets  CB2  low  (for  -busy  to  sending  port)  */ 
void  rst_bsyA (base) 

register  struct  MFI_PIA  *base;  /*  base  address  of  MFI  */ 

{ 

register  short  temp; 

if  (LOCATIONLAB)  /*  in  virtual  world,  do  not  read  any  slots  */ 

return; 
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} 

temp  =  (base“>cra  &  OxFF) ;  /*  save  era  values  */ 

base->cra  =  0x30;  /*  8  bit  0=  CR2  low  */ 

base~>cra  =  temp;  /*  restore  era  values  */ 

} 

/*  --  NOT  YET  UPDATED  TO  CURRENT /OPERATIONAL  MARCO  AUV  HARDWARE /SOFTWARE  --  */ 
/*  not  found!  */ 
int  ek_sta  (base) 

register  struet  MFI_PIA  *base;  /*  base  address  of  MFI  */ 

{ 

register  unsigned  short  temp; 


if  (LOCATIONLAB)  /*  in  virtual  world,  do  not  read  any  slots  */ 

{ 

return  ( 0 ) ; 

} 

temp  =  base~>cra;  /*  save  era  values  */ 

return  (temp) ; 


/*******★****************************★**************************************/ 

char  send_sonar_command  (coinmand__char) 
char  command_char; 

{ 

/*  This  function  sends  a  single  character  and  reads  back  a  single 
character  from  the  sonar  */ 

unsigned  n,n_bytes; 

char  reply , sonar_read [ 20 ] , command [ 1 ] ; 
command [0]  =  command_char; 

if  (TRACE  DISPLAYSCREEN) 

printf  ("[Begin  send_sonar_command  ()]\n"); 

n  =  write (sonarpath, command, 1) ; 
tsleep (3 ) ; 

n_bytes  =  _gs_rdy(sonarpath); 

if  (n_bytes  ==  1) 

{ 

n  =  read { sonarpath, sonar_read,n_bytes) ; 

} 

reply  =  sona reread [0] ; 

if  (TRACE  &&  DISPLAYSCREEN) 

printf  ("  [End  send_sonar_coinmand  ( )  ]  \n"  )  ; 

return (reply) ; 

}  /*  end  send_sonar_command  ()  */ 

/********★*★★**************★*****★***********************************★******/ 

void  set_stcp_size (step_code) 
int  step_code; 

{ 

unsigned  n,n_bytes; 

char  reply , sonar_read[ 20 ] , command [1] ; 

if  (TRACE  &&  DISPLAYSCREEN) 

printf  ("[Begin  set_step_size]  \n"  )  ,* 

switch {step_code) 

{ 
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case  1 : 

coiimiandfO]  =  'H';  /*  0.9  degrees  for  st725  I  don't  know  for  stlOOO  */ 

SONARHEADINGSTEP  =  0.9; 

break; 

case  2: 

coimnand[0]  =  '1';  /*  1.8  degrees  for  stlOOO  I  think  Y  for  st725  */ 

SONARHEADINGSTEP  =  1.8; 

break; 

case  4: 

coiTiinand[0]  =  "2';  /*  3.6  degrees  */ 

SONARHEADINGSTEP  =  3.6; 
break; 

} 

if{step_code  ==  0) 

{ 

printf ( "*****  Step  Code  Is  0  ******\n"); 

} 

else 

{ 

write  (sonarpath,coiTiinand,  1)  ; 
t sleep (5) ; 

n_bytes  =  _gs_rdy (sonarpath) ; 

if{n_bytes  ==  1) 

{ 

n  =  read(sonarpath,sonar_read,n_bytes) ; 

} 

} 

if  (TRACE  ScSc  DISPLAYSCREEN)  printf  ("[End  set_step_size]\n"); 
return; 

}  /*  end  set_step_size  ()  */ 

/***************************************************************************/ 
void  initialize_sonar  () 


int  TRACE  =  TRUE; 

int  gain  =  25;  /*  value  from  0  to  100  */ 

int  max_range  =  10;  /*  others:  1,2,4,6,10,20,25,30,50,100  meters.*/ 
unsigned  n_bytes; 

unsigned  short  Tl, T2 ,T3 , Tchecksum; 

char  command [1] , sonar_read [20] , *t, sonar_write [1] , reply; 
unsigned  short  by te, by tel, Nsampl,Nbins,Range_Code, checksum; 
unsigned  short  TxPulseMSByte,TxPulS€LSByte,Gecmin, Rng_unt; 
int  i, j , k, n, word,TxPulse; 

int  Timout ,Lokout, Eswait,Gaindt, Ecsclx, Ecscly; 
int  Maxdst , Dacscx, Dacscy ; 
int  EchoSounder [ 10 ] ; 

if  (SONARINSTALLED  ==  FALSE)  return; 
if  (TRACE  ScSc  DISPLAYSCREEN) 

{ 

printf (" [Begin  initialize_sonar] \n" ) ; 
printf ("Sonar  max_range  =  %d\n" ,max_range) ; 
printf ("Sonar  gain  =  %d\n",gain); 

} 
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INITIALIZATION  PARAMETERS  FOR  PROFILING  MODE  (ST-IOOO  HEAD) 


Range 

meters 

1 

1 

1 

TxPulse 

1 

} 

1 

NSAMPL 

1 

1 

1 

NBINS 

1 

( 

1 

Range  Code 

1 

i 

I 

TIMOUT 

i 

1 

I 

Maxdst 

1 

I 

1 

30 

1 

! 

1 

1 

1 

64 

I 

1 

00 

1 

1 

1500 

1 

1 

1500 

2 

1 

30 

1 

1 

1 

64 

1 

01 

1 

3000 

i 

3000 

4 

1 

30 

! 

1 

1 

128 

i 

02 

1 

6000 

! 

6000 

6 

1 

30 

i 

1 

1 

128 

1 

03 

1 

9000 

1 

9000 

10 

1 

40 

1 

1 

I 

128 

1 

04 

i 

15000 

1 

15000 

20 

1 

50 

j 

3 

1 

128 

1 

05 

1 

30000 

1 

30000 

30 

1 

75 

i 

6 

i 

128 

1 

06 

1 

45000 

1 

45000 

50 

1 

100 

1 

12 

I 

128 

1 

07 

1 

65535 

1 

65535 

EC PULS  =30 
LOKOUT  =  200 
ESWAIT  =  25600 
GECMIN  =  Byte 
GAINDT  =64 
ECSCLX  =  16383 
ECSCLY  =  11374 
DACSCX  =256 
DACSCY  =3125 
Rng  Unt  =  1 

above  for  all  ranges. 

*/ 

switch (max^range) 

{ 

case  1: 

TxPulse  =  30; 

Nsampl  =  1 ; 

Nbins  =  64; 

Rang encode  =  0 ; 

Timout  =  1500; 

Maxdst  =  1500; 

break; 

•  case  2 : 

TxPulse  =  30; 

Nsampl  =  1; 

Nbins  =  64;  . 

Rang encode  =  1 ; 

Timout  =  3000; 

Maxdst  =  3000; 

break; 


case  4: 


TxPulse 

= 

30; 

Nsampl 

1; 

Nbins 

128; 

Range_Code 

= 

2; 

Timout 

6000; 

Maxdst 
break ; 

6000; 

:ase  6: 

TxPulse 

= 

30; 

Nsampl 

= 

1; 

Nbins 

= 

128; 

Rang encode 

3; 

Timout 

9000; 

Maxdst 

= 

9000; 
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break; 


case  10: 


TxPulse 

40; 

Nsampl 

1; 

Nbins 

= 

128; 

Range__Code 

= 

4; 

Timout 

= 

15000 

Maxdst 

= 

15000 

break ; 

case  20: 

TxPulse 

= 

50; 

Nsampl 

= 

3; 

Nbins 

=: 

128; 

Rang encode 

=r 

5; 

Timout 

30000 

Maxdst 

= 

30000 

break ; 

case  30: 

TxPulse 

75; 

Nsampl 

= 

6; 

Nbins 

= 

128; 

Rang encode 

= 

6; 

Timout 

= 

45000, 

Maxdst 

= 

45000, 

break ; 

case  50: 

TxPulse 

s: 

100; 

Nsampl 

= 

12; 

Nbins 

=: 

128; 

Range_Code 

= 

7; 

Timout 

= 

65535; 

Maxdst 

65535; 

break ; 
default : 
break;  . 

}  /*  End  switch  */ 


/*  Values  Coinmon  to  all  Ranges  */ 
Lokout  =  200; 

Eswait  =  25600; 

Gecmin  =  2.55*gain; 


Gaindt 

Ecsclx 

Ecscly 

Dacscx 

Dacscy 

Rng_unt 


=  64; 

=  16383; 
=  11374; 
=  256; 

=  3125; 

=  1; 


/*  Send  Sonar  Parameters  */ 

command [0]  =  'P'; 

write (sonarpath, command, 1) ; 


/*  Send  TxPulse  Length  (Word)  in  1.96  usee  units  */ 
word  =  TxPulse  &  OxOOff;  /*  Byte  =  LSByte  of  TxPulse  */ 
byte  =  word; 

TxPulseLSByte  =  byte; 

sonar_write[03  =  (char)  byte; 

n  =  write(sonarpath,sonar_write, 1) ;  /*  Send  LSByte  First  */ 
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word  =  TxPulse  »  8;  /*  Byte  =  MSByte  of  TxPulse  */ 

byte  =  word; 

TxPulseMSByte  =  byte; 
sonar_write[0]  =  (char)  byte; 

n  =  write (sonarpath, sonar_write, 1) ;  /*  Send  MSByte  Last  */ 

/*  Send  NSAMPL  (Byte)  NO.  A/D  Samples  per  Bin  */ 
byte  =  Nsampl; 

sonar_write [ 0]  =  (char)  byte; 
n  =  write (sonarpath, sonar_write, 1) ; 

/*  Send  NBINS  (Byte)  No.  of  Bins  to  Collect  */ 
byte  =  Nbins; 

sonar_write[0]  =  (char)  byte; 
n  =  write (sonarpath, sonar_write,  1) ; 

/*  Send  Range  Code  0-8  (obsolete)  (Byte)  */ 
byte  =  Range_Code; 
sonar_write[0]  =  (char)  byte; 
n  =  write(sonarpath,sonar_write,l)  ; 

/*  Send  DataByte  checksum  (Byte) .  Should  be  the  lowest  8  bits  of  */ 

/*  the  sum  of  all  Bytes  */ 

word  =  TxPulseMSByte  +  TxPulseLSByte  +  Nsampl  +  Nbins  +  Range_Code; 
word  =  word  &  OxOOff;  /*  Mask  MSByte  to  get  last  8  bits  for  checksum;  */ 
checksum  =  word; 

/*printf ("ckecksum  =  %d\n" , checksum) ; */ 
byte  =  checksum; 
sonar_write(0]  =  (char)  byte; 
n  =  write(sonarpath,sonar_write,l)  ; 

tsleep (5) ; 

n_bytes  -  _gs_rdy (sonarpath) ; 

/*  Read  Reply  to  Checksum  */ 
n  =  read(sonarpath,sonar__read,  1)  ; 
reply  =  sona reread [0 J ; 
if  (.-reply  ==  'T') 

{ 

if  (TRACE  &&  DISPLAYSCREEN)  printf ( "Sonar  Parameter  Checksum  Ok\n"); 

else 

{ 

if  (TRACE  &&  DISPLAYSCREEN) 

printf ("Sonar  Parameter  Checksum  INCORRECT' I i\n"); 


/*  Enable  TVG  should  reply  'X'  */ 
reply  =  send_sonar_command( 'X' )  ; 
if (reply  ==  'X') 

{ 

if  (TRACE  ScSc  DISPLAYSCREEN)  print f  ( "Sonar  TVG  set\n")  ; 

} 

else 

{ 

if  (TRACE  &&  DISPLAYSCREEN)  printf ( "Sonar  TVG  not  set!\n"); 


/*  Set  mode  return  Range  bin  Peak  should  reply  'K'  */ 
reply  =  send_sonar_command ( 'K' ) ; 
if (reply  ==  'K' ) 

{ 

^if  (TRACE  &&  DISPLAYSCREEN)  printf  ("Sonar  Range  bin  Peak  mode  Ok\n")  ; 

else 

{ 

if  (TRACE  &&  DISPLAYSCREEN) 
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printf ("Sonar  Range  bin  Peak  mode  not  set!\n"); 

} 

/*  Set  Final  Gain  for  TVG,  should  reply  'E'  */ 
reply  =  set_scanning_gain(83 , 'E' ) ; 
if (reply  ==  'E') 

{ 

if  (TRACE  ScSc  DISPLAYSCREEN)  printf  ( "Sonar  Final  TVG  Gain  set\n"); 

else 

{ 

if  (TRACE  &&  DISPLAYSCREEN)  printf ( "Sonar  Final  TVG  Gain  not  set!\n") 


EchoSounder [ 0] 
EchoSounder [ 1 ] 
EchoSounder [2] 
EchoSounder [3] 
EchoSounder [ 4] 
EchoSounder ( 5 ] 
EchoSounder [6] 
EchoSounder [7] 
EchoSounder [8] 
EchoSounder [9] 


=  75; 

=  Timout; 
=  Lokout; 
=  Eswait; 
=  Gaindt; 
=  Ecsclx; 
=  Ecscly; 
=  Maxdst; 
=  Dacscx; 
=  Dacscy; 


checksum  =  0 ; 


/*  Send  Profiler  Sonar  Parameters  */ 

command [0]  =  'J'; 

write (sonarpath, command,!) ; 


/*  Send  First  4  Parameters  (Words)  */ 
for (i=0;i<4;++i) 

{ 

word  =  EchoSounder [ i ]  &  OxOOff;  /*  Byte  =  LSByte  of  EchoSounder [i]  */ 
byte  =  word; 

checksum  =  checksum  +  byte;  /*  Add  up  the  checksum  */ 
sonar_write[0]  =  (char)  byte; 

n  =  write  (sonarpath,sonar_write,l) ;  /*  Send  LSByte  First  */ 

word  =  EchoSounder [i]  »  8;  /*  Byte  =  MSByte  of  EchoSounder [i]  */ 

byte  =  word; 

checksum  =  checksum  +  byte;  /*  Add  up  the  checksum  */ 
sonar_write[0]  =  (char)  byte; 

n  =  write  (sonarpath,  sonar__write,  1)  ;  /*  Send  MSByte  Last  */ 


/*  Send  Gecmin  (Byte)  */ 

byte  =  Gecmin; 

checksum  =  checksum  +  byte; 

sonar_write [0 ]  =  (char)  byte; 

n  =  write (sonarpath,sonar_write,l); 


/*  Send  Last  6  Parameters  (Words)  */ 
for (i=4;i<10;++i) 

{ 

word  =  EchoSounder [i]  &  OxOOff;  /*  Byte  =  LSByte  of  EchoSounder [i ]  */ 
byte  =  word; 

checksum  =  checksum  +  byte;  /*  Add  up  the  checksum  */ 
sonar__write[0]  =  (char)  byte; 

n  =  write  (sonarpath,  sonar_write,  1) ;  /*  Send  LSByte  First  */ 

word  =  EchoSounder [i]  »  8;  /*  Byte  =  MSByte  of  EchoSounder [ i ]  */ 

byte  =  word; 

checksum  =  checksum  +  byte;  /*  Add  up  the  checksum  */ 
sonar_write[0]  =  (char)  byte; 

n  =  write (sonarpath, sonar_write, 1) ;  /*  Send  MSByte  Last  */ 
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} 


/*  Send  Rng_unt  (Byte)  */ 

byte  =  Rng_unt; 

checksum  =  checksum  +  byte; 

sonar_write [0]  =  (char)  byte; 

n  =  write (sonarpath, sonar_write, 1 ) ; 

/*  Send  DataByte  checksum  (Byte) .  Should  be  the  lowest  8  bits  of  */ 

/*  the  sum  of  all  Bytes  */ 

checksum  =  checksum  &  OxOOff;  /*  Mask  MSByte  to  get  last  8  bits  */ 

if  (TRACE  ScSc  DISPLAYSCREEN) 

printf ("Sonar  Profile  checksum  =  %d\n" , checksum) ; 
byte  =  checksum; 
sonar_write[0]  =  (char)  byte; 
n  =  write (sonarpath/ sonar_write, 1) ; 

tsleep (5) ; 

n_bytes  =  _gs_rdy (sonarpath) ; 

/*  Read  Reply  to  Checksum  */ 
n  =:  read(sonarpath,sonar_read,  1)  ; 
reply  =  sonar_read[0] ; 
if (reply  ==  'T') 

{ 

if  (TRACE  ScSc  DISPLAYSCREEN)  printf  ( "Sonar  Profile  Checksum  Ok\n"); 

} 

else 

{ 

if  (TRACE  &&  DISPLAYSCREEN)  printf ( "Sonar  Profile  Checksum  IncorrectXn" ) 


/*  Check  if  head  is  using  default  settings.  Reply  is  'T'  if  yes,  */ 
/*  'F'  if  not  */ 

reply  =  send_sonar_command ( 'D' ) ; 
if (reply  ==  'T' ) 

{ 

if  (TRACE  ScSc  DISPLAYSCREEN) 

printf ("Sonar  Head  is  still  using  Default  Settings i \n") ; 

else 

{ 

if  (TRACE,  &&  DISPLAYSCREEN) 

printf ("Sonar  Head  not  using  Default  SettingsNn" ) ; 

} 

if  (TRACE  ScSc  DISPLAYSCREEN)  printf  ("[End  initialize_sonar  ]  \n" )  ; 
return; 


/******************** *******************************************************^ 
char  set  ,scanning_..gain  (gain,which_gain) 
int  gain; 

char  which_gain;  /*  which_gain  =  'B'  for  Initial,  'E'  for  Final  */ 

{ 

int  TRACE  =  TRUE; 

unsigned  short  byte; 
unsigned  n,n_bytes; 

char  reply , sonar_write [1 ] , sonar_read [20] , command [1] ; 

if  (TRACE  ScSc  DISPLAYSCREEN) 

printf  ("[Begin  set_scanning_gain] \n" ) ; 
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/*  Set  Initial  or  Final  Gain  for  TVG  should  reply  */ 
/*  'C'  for  Initial  or  'E'  for  Final  */ 
coininand[0]  =  which_gain; 
wri  te  ( sonarpath,  coinmand,  1 )  ; 
byte  =  2.55*gain; 
sonar_write[0]  =  (char)  byte; 
n  =  write (sonarpath, sonar_write, 1) ; 
tsleep (5) ; 

n_bytes  =  _gs_rdy  (sonarpath)  ; 
read  ( sonarpa  th ,  sonar_read,  n_by  tes )  ; 
reply  =  sonar_read [0 ] ; 

if  (TRACE  &&  DISPLAYSCREEN) 

printf  ("[End  set_scanning_gain]\n"); 

return (reply ) ; 


/***★**********★****************************************************★**★****/ 

void  center_sonar  () 

{ 

int  TRACE  =  TRUE; 

int  n,  n_bytes,  encoder_width,  count; 

char  encoder- 

char  sonar_read[200]  ; 

if  (SONARINSTALLED  ==  FALSE)  return; 

if  (TRACE  &&  DISPLAYSCREEN) 

printf  ("[Begin  center_sonar] \n") ; 

AUV_ST1000_bearing  =  0.0; 

if  (LOCATIONLAB  ==  FALSE) 

{ 

encoder_width  =  0; 
n_bytes=_gs_rdy  (sonarpath)  ; 

/*  Flush  the  Buffer  to  Ensure  a  Clean  Start  */ 
if(n_bytes  !=  ~1) 

{ 

if  (TRACE  &&  DISPLAYSCREEN) 

printf ("n_bytes  in  sonar  buffer  =  %d\n" ^n^bytes) ; 
n=  read  (sonarpath,  sonar__read,  n_bytes)  ; 

) 

/*  Clear  out  any  junk  from  buffer  at  startup  */ 
while (encode  I-  '3') 

{ 

encode  =  send_sonar_command( "V' ) ; 

} 

/*  Get  Outside  Encoder  */ 
do  { 

encode  =  send_sonar_command  ('  +  '); 

}  while  (  (encode  *!=  'f')  ScSc  (encode  !=  'F')); 

if  (DISPLAYSCREEN)  printf (" [Found  STIOOO  Non-Encoder  Space] \n"); 

/*  Find  Encoder  Edge  */ 
do  { 

encode  =  send_sonar_command( ' - ' ) ; 

}  while  ((encode  't')  &&  (encode  1=  'T')); 
encode  =  send_sonar_coinmand  ('  +  '); 

if  (DISPLAYSCREEN)  printf (" [Found  STIOOO  Encoder  Edge]\n"); 
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/*  Sweep  Across  Encoder  To  Determine  Width  */ 
encoder_width  =  0 ; 
do  { 

encode  =  send_sonar_coinmand 
encode r_width++; 

}  while  ((encode  !=  'f')  &&  (encode  i=  "F')); 
if  (DISPLAYSCREEN) 

printf (" [STIOOO  Encoder  Width  Determined:  %dj \n" , encoder_width) ; 

/*  Sweep  Back  Halfway  Into  Encoder  */ 
for  (count  =  0;  count  <  encoder_width  /  2;  count++) 
encode  =  send__sonar_command  ('  +  '); 
if  (DISPLAYSCREEN)  printf (" [STIOOO  Center  Established] \n"); 


if  (TRACE  &&  DISPLAYSCREEN)  printf  ("[End  center^sonar] \n" ) ; 


return; 

)*************** 


void  step_sonar  (direction) 
int  direction; 


{ 

if  (SONARINSTALLED  ==  FALSE) 


return; 


if  (TRACE  ScEc  DISPLAYSCREEN) 

printf  ("[Begin  step_sonar] \n" ) ; 

if  (direction  ==  LEFT) 

AUV_ST1000_bearing  =  normalize  (AUV_ST1000_bearing  -  SONARHEADINGSTEP) ; 
else  if  (direction  ==  RIGHT) 

AUV_ST1000_bearing  =  normalize  (AUV_ST1000_bearing  +  SONARHEADINGSTEP); 

if  ( (AUV_ST1000_bearing  <  0.9)  M  (AUV_ST1000_bearing  >  359.1)) 
AUV_ST1000_bearing  =  0.0; 

if  (TRACE  Sc&  DISPLAYSCREEN) 

{ 

printf  ("[Step  Direction:  %dl] \n" , direction) ; 

printf  (" [New  STIOOO  Bearing;  %6 .21f ] \n",AUV_ST1000_bearing) ; 


} 


if  (TRACE  ScSc  DISPLAYSCREEN) 

printf  ("[End  step_sonar] \n") ; 


/**************-k*-k*icic*ic***ic*ic****ic****icic*-kicic*icic*-kic-kic*icic**icic********ic**icic-k**ic/ 

void  ping_sonar  (direction) 
int  direction; 

{ 

int  i^n^n^bytes; 

char  command [1] ,sonar_read [20] ; 

if  (SONARINSTALLED  ==  FALSE)  return; 

if  (TRACE  ScSc  DISPLAYSCREEN) 

printf  ("[Begin  ping_sonar] \n" ) ; 

if  (LOCATIONLAB  ==  FALSE) 

{ 

/*  Clear  Out  Any  Data  That  May  be  On  Port  Prior  to  Write  */ 
n_bytes  =  _gs_rdy(sonarpath); 
if{n_bytes  1=  -1) 

{ 

if  (TRACE  &&  DISPLAYSCREEN) 

printf (" [Clearing  %d  junk  bytes  before  write  to  sonar] \n",n_bytes); 
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n=  read ( s  onarpa  th , sonar_r ead, n_by  tes ) ; 

} 

if (direction  ==  0) 

{ 

if  (TRACE  &&  DISPLAYSCREEN) 

printf (" [Sonar  Ping  Only]\n"); 
command [0]  -  'Z' ; 

/*  Global  Variable  that  indicates  how  many  bytes  to  expect  */ 
stl000_bytes_expected  =  2; 

} 

else  if(  (direction  ==  LEFT)  M  (direction  ==  RIGHT)  ) 

if  (TRACE  &&  DISPLAYSCREEN) 
if  (direction  ==  LEFT) 

printf (" [Ping  sonar  and  step  left]\n"); 
else  printf (" [Ping  sonar  and  step  right] \n"); 

/*  STIOOO  mounted  upside  down  on  bottom,  left  &  right  reversed  */ 
if (direction  ==  LEFT)  command[0]  =  ')'; 
if (direction  ==  RIGHT)  command [0]  =  '('; 

/*  Global  Variable  that  indicates  how  many  bytes  to  expect  */ 
stl000_bytes_expected  =  3; 

} 

else 

{ 

printf (" [Invalid  direction,  command  ignored! ] \n" ) ; 
return; 

} 

if  (TRACE  &&  DISPLAYSCREEN) 

printf (" [Performing  write  to  sonar]\n"); 
write (sonarpath,  command,  1); 

} 

if  ((direction  ==  LEFT)  M  (direction  ==  RIGHT)  II  (direction  ==  0)) 

SONARPINGED  =  TRUE; 
step_sonar  (direction) ; 

} 

if  (TRACE  &&  DISPLAYSCREEN) 

printf  ("[End  ping_sonar] \n") ; 

return; 

}  /*  end  ping_sonar  ()  */ 

/****ic*******-kicicic’kic**************ic*ic-k*****icicic-kic**icic*****-kicicic****-k*ic***ici,icicic*/ 

double  read_sonar  {) 

{ 

/*  int  TRACE  =  TRUE;  */ 

int  i,n,n_bytes; 
int  RESET_PORT; 

char  sonar_read[200] ,sonar_read2 [1] , *ttt; 
unsigned  short  profilebyte; 
unsigned  short  timeout; 

if  (SONARINSTALLED  ==  FALSE)  return  0.0; 

if  (TRACE  ScSc  DISPLAYSCREEN) 

printf  {"[Begin  read_sonar] \n" ) ; 

if  (LOCATIONLAB  ==  FALSE) 

{ 

ttt  =  (char  *)  &  profilebyte; 
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n_bytes  =  _gs_rdy (sonarpath) ; 


timeout  =  0; 

RESET_PORT  =  FALSE; 

/*  Loop  Until  Data  is  There  */ 
while  (n_bytes  <  stl000_bytes__expected) 

{ 

tsleep (2 ) ; 

n_bytes  =  _gs_rdy  (sonarpath)  ; 
if  (TRACE  ScSc  DISPLAYSCREEN) 

printf ("STIOOO  bytes  ready  =  %d\n" , n„bytes) ; 
if  ((timeout  ==  2)  &&  (n__bytes  !=  stl000_bytes__expected) ) 
{ 

RESETJORT  =  TRUE; 
break; 

} 

timeout++; 


if (RESET_PORT) 

{ 

if  (TRACE  ScSc  DISPLAYSCREEN)  printf  ( "  [ * **Sonar  Port  Reset***]\n"); 
AUV_ST1000_range  =0.0; 
tty_mode (sonarpath, 0 ) ; 
close (sonarpath) ; 

sonarpath  =  open {"/t3" , S_IWRITE+S_IREAD) ; 
tty_mode (sonarpath, 1 ) ; 
n_bytes  =  _gs_rdy (sonarpath) ; 

if(n_bytes  !=  -1  )  n=read (sonarpath, sonar_read, n_bytes ); /*Read  Junk  */ 
n_bytes  =  -1; 

} 

if(n_bytes  ==  stl000_bytes_expected) 

{ 

if  (TRACE  ScSc  DISPLAYSCREEN) 

printf ("STIOOO  correct  #  of  bytes  =  %d\n", n_bytes) ; 

n=read { sonarpath, sonar_read, 1 ) ; 
ttt[l]  =  sonar_read[0] ; 
n=read ( sonarpath, sonar_read, 1) ; 
ttt[0]  =  sonar_read[0] ; 

if (stl000_bytes_expected  ==  3)  n= read (sonarpath, sonar_read2 , 1 ) ; 

/*Read  TtFf*/ 

AUV_ST1000_range  =  0 , 0328*prof ilebyte; 

} 

if(n_bytes  >  stl000_bytes_expected) 

{ 

/*  Incorrect  number  of  bytes  in  buffer,  clear  it  outi  */ 
if  (TRACE  ScSc  DISPLAYSCREEN) 

printf (" [***ST1000  Incorrect  #  of  bytes  =  %d***] \n" ,n_bytes) ; 

n=read (sonarpath, sonar_read, n_bytes) ; 

AUV_ST1000_range  =  0.0; 


} 

if  {AUV_ST1000_range  >  32.808)  AUV_ST1 00 0_ range  =  0.0; 

if  (TRACE  ScSc  DISPLAYSCREEN) 

{ 

printf  (" [AUV_ST1000_range  =  %5 . 31f ] \n" , AUV_ST1000_range) ; 
printf  {" [End  read_sonar] \n") ; 

} 

return  (AUV_ST1000_rangG)  ; 
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/******************** ***************************************^***************^ 
void  control_sonar  ()  /*  STIOOO  execution  level  (ST725  is  tactical  level)  */ 


static  double  previous_range  =  0.0; 
static  double  start_bearing  =  0.0; 

static  double  end_bearing  =  0.0; 

static  double  range_accumulator  =  0.0; 
static  int  valid__return_count  =  0; 
static  int  no_return_count  =  ec¬ 
static  int  scan_onto_target  =  FALSE; 
static  int  update_target_data  =  FALSE; 


double  new_target_bearingc 
new_target_range, 
commanded_bearing_errorc 
sonar_return_x, 
s on ar_r e t u r n^y ; 

if  {SONARINSTALLED  ==  FALSE)  return; 

if  (TRACE  SeSe  DISPLAYSCREEN) 

printf  (" [Begin  control_sonar] \n") ; 

/*************************************************** 

/*  Sonar  Scan  Modes 
/* 

/*  1:  normal  forward  scan  (SCANWIDTH/2 . 0  degrees  either  side) 

/*  2:  target  edge  scan  {either  left  or  right) 

/*  3:  full  target  scan 
/*  4:  locate  target 
/*  5:  manually  position 
/* 

/*************************★*************************/ 
switch  (SONARSCANMODE) 

{ 

case  (1):  /*  Normal  Straight  Ahead  Scan  Mode  to  Detect  Collisions  */ 

if  (TRACE  SeSe  DISPLAYSCREEN) 

printf  ("[Sonar  using  normal  forward  scanjXn"); 

/*  See  if  it  is  time  to  reverse  scan  direction  */ 

if  ( ( (SONARSCANDIRECTION  ==  RIGHT)  li  (SONARSCANDIRECTION  ==  0 ) )  && 
(normali2e2  (AUV_ST1000__bearing)  >=  normalize2  (SCANWIDTH/2 . 0) ) ) 
SONARSCANDIRECTION  =  LEFT; 

else  if  ({(SONARSCANDIRECTION  ==  LEFT)  il  (SONARSCANDIRECTION  ==  0))  && 
(normalizG2  (AUV_ST1000_bearing)  <=  -normalize2  (SCANWIDTH/2 . 0) ) ) 
SONARSCANDIRECTION  =  RIGHT; 
else  if  (SONARSCANDIRECTION  ==  0) 

SONARSCANDIRECTION  =  RIGHT; 

ping_sonar  (SONARSCANDIRECTION)  ; 

break; 

case  (2):  /*  Edge  Tracking  Mode  */ 

if  (TRACE  SeSe  DISPLAYSCREEN) 

printf  ("[Sonar  in  edge  tracking  mode]\n"); 

/*  Decide  whether  or  not  ping  was  on  the  target  or  not  */ 

if  ( (ST1000__range_kal  >  0.0)  &&  /*  fabs  needed  next  line  ??  */ 

(fabs  (ST1000_range_kal  -  previous_range)  <  3.0)) 

{ 

if  ( ( (valid_return_count  ==  0)  &&  (scan_onto_targGt ) )  II 
(scan_onto__target  ==  FALSE)  ) 
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{ 


start_bearing 


=  normalize  (psi  +  AUV_ST1000_bearing) ; 


} 

previous_range  =  ST1000_range_kal; 

no_return_count  =  0; 

range__accuinulator  +=  ST1000_range_kal; 

va 1 i d_r e tu rn_coun t + + ; 

} 

else 

{ 

n  o_r  e  tu  rn_c  ou  n  t + +  ; 

} 

/*  Check  to  see  if  it  is  time  to  reverse  the  scan  and  compute  numbers  */ 
i f  { scan_onto_target ) 

{ 

if  ( (valid_return_count  >2)  II 

( (valid__return_count  >  0)  &&  {no_return__count  >2))) 

{ 

update_target_data  =  TRUE; 
scan_onto_target  =  FALSE; 

} 

} 

else 

{ 

if  (no_rGturn_count  >  2) 

{ 

update_target_data  =  TRUE; 
scan_onto_target  =  TRUE; 

} 

} 

if  (fabs(normalize2 (normalize (AUV_ST1000_bearing+psi )“target_bearing) )  >  90.0) 

{ 

if  (TRUE  ScSc  DISPLAYSCREEN) 

printf  ("[Lost  Target,  Switching  to  Target  Search  Sonar  Mode]\n"); 
scan_o‘nto_target  =  TRUE; 

SONARSCANDIRECTION  =  -SONARSCANDIRECTION; 

SONARSCANMODE  =  4  ; 

} 

if  {update_target_data) 

{ 


if  (valid_return_count  >  0) 

{ 

/*  Compute  a  sonar  range  and  bearing  */ 
target_bearing  =  start_bearing; 

new_target_range  = 

range_accumulator  /  (double)  valid_return_count; 

/*  Determine  location  of  target  in  world  coordinates  */ 
target_x  =  x  +  cos_psi  *  AUV_ST1000_x_of f set 
/*  X  position  of  sonar  */ 

-  sin_psi  *  AUV__ST1000_y_of f set ; 
target_y  =  y  +  sin_psi  *  AUV_ST1000__x_of f set 
/*  y  position  of  sonar  */ 

+  cos_psi  *  AUV_ST1000_y_of f set; 
targe t_x  =  targe t_x 

+  cos  (radians  (target_bearing) )  *  new_target_range; 
target_y  =  target_y 

+  sin  (radians  ( targe t_bearing) )  *  new_target_range; 

/*  Determine  location  of  vehicle  center  relative  to  target  */ 
target_bearing  =  normalize  (degrees  (atan2z  (  (target__y  -  y) , 

( targe t_x  -  x)))); 

new_target_range  =  sqrt  ( (x  -  target_x)  *  (x  ~  target_x)  + 

(y  -  target_y)  *  (y  -  target_:y) )  ; 
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if  (TRACE  ScSc  DISPLAYSCREEN) 

{ 

print f  ("[Target  X  Location:  %8.31f]\n",  target^x); 
printf  (" [Target  Y  Location:  %8.31f]\n",  target_y); 
printf  ("[Target  Bearing:  %8.31f]\n",  target_bearing) 
printf  (" (Target  Range:  %7.31f]\n",  target_range) ; 


target_range  = 

new_target_update  = 

if  (scan_onto_target ) 

{ 

range_accumu 1 a  to  r 
va  1  i  d_re  tu  rn_c  ou  n  t 

} 

else 

{ 

range_accumulator  = 

valid_return_count  = 

} 

no_return_count  = 

SONARSCANDIRECTION  *= 

tiine_last_target_update  = 


new_ta rget_range ; 
TRUE; 


=  0.0; 

=  0; 


previ  ous_range  ; 
1; 


0; 

-1.0; 

t; 


} 


fprintf  (targetdatafile, "%lf  %lf  %lf  %lf  %lf  %lf  %lf\n", 
t,  target_range,  target__bearing,psi, 
target_range_coininand,  targe  t_bearing_coinmand, 
psi_coiTimand_tgt)  ; 
if  (TRUE  &&  DISPLAYSCREEN) 

printf  {"[Target  Update:  %5.21f  %5.21f]\n", 
target_range,  target_bearing) ; 


update_target_data  =  FALSE; 

} 

if  { (scan_onto_target)  M  (no_return_count  ==  0}) 
ping_sonar  (SONARSCANDIRECTION) ; 
else  ping_sonar  (0); 

break; 

case  (3):  /*  Target  Tracking  Mode  */ 

if  (TRACE  &&  DISPLAYSCREEN) 

printf  ("[Sonar  in  target  tracking  mode]\n"); 

/*  See  if  it  is  time  to  reverse  scan  direction  */ 
if  {  (ST1000__range_kal  >  0.0)  && 

( (ST1000_range_kal  -  previou strange)  <=5.0)) 

{ 

end_bearing  =  normalize  (psi  +  AUV_ST1000_bearing) ; 
if  (valid_return_count  ==  0) 
start_bearing  =  end_bearing; 
previou  strange  =  ST1000_range_kal; 

no_return__count  =  0; 

range_accumulator  +=  ST1000_range_kal; 

valid_return__count  +=  1; 

ping_sonar  (SONARSCANDIRECTION) ; 

} 

else 

{ 

if  (no_return_count  >  3)  /*  scanned  off  end  of  target  */ 

{ 

if  {valid_return_count  >  0) 

{ 

/*  Compute  a  sonar  range  and  bearing  */ 
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new__target_bearing  =  normalize  (start^bearing  + 

nonnalize2  (end_bearing  -  start_bearing)  /  2.0); 
new__target_range  = 

range_accumulator  /  (double)  valid_return_count ; 

/*  Determine  location  of  target  in  world  coordinates  */ 
target_x  =  x  +  cos_psi  *  AUV_ST1000_x_of fset 
-  sin_psi  *  AUV_ST1000_7_offset; 

/*  X  position  of  sonar  */ 
target_7  =  y  +  sin_psi  *  AUV_ST1000_x_of fset 
+  cos_psi  *  AUV_ST1000_y_of fset; 

/*  y  position  of  sonar  */ 
target_x  =  targe t_x 

+  cos  (radians  (new_target_bearing) ) 

*  new__target_range; 
target_y  =  target_y 

+  sin  (radians  (new_target_bearing) ) 

*  new_target_range; 

/*  Determine  location  of  vehicle  center  relative  to  target  */ 
target_bearing  =  normalize  (degrees  (atan2z  ((target^  -  y), 

(target_x  -  x) ) ) ) ; 

new_target_range  =  sqrt  ( (x  -  target_x)  *  (x  ~  target__x)  + 

(y  -  target^/)  *  (y  -  target_y)); 

if  (TRACE  &&  DISPLAYSCREEN) 

{ 

printf  ("[Target  X  Location:  %8.31f]\n",  target__x).; 
printf  ("[Target  Y  Location:  %8.31f]\n",  target_y) ; 
printf  ("[Target  Bearing:  %8.31f]\n",  target_bearing) ; 
printf  (" [Target  Range:  %7.31f]\n",  target_range) ; 

} 

target_range  =  new__target__range; 

new__target_update  =  TRUE; 

printf  ("[Target  Update:  %5.21f  %5.21f]\n", 
targe t_range,  targe t_bearing) ; 
fprintf  (targetdatafile, "%lf  %lf  %lf  %lf  %lf  %lf  %lf\n", 
t , target_range, target_bearing,psi , 
targe t_range_command,  target_bearing_coinmand, 
psi_command_tgt)  ; 
time__last_target_update  =  t; 


new_target_update  =  TRUE; 

no_return_count  =  0; 

valid_return_count  =  0; 

range_accumulator  =  0.0; 

SONARSCANDIRECTION  *=  -1; 


ping_sonar  (SONARSCANDIRECTION) ; 

} 

else  /*  backtrack  if  scanned  more  than  one  step  off  target  */ 
{ 

ping_sonar  (SONARSCANDIRECTION); 
no_return_count  =  0; 

} 

} 

else 

{ 

ping_sonar  (0); 

++no_return_count ;  /*  try  again  before  reversing  scan  */ 


} 

break; 

case  (4) :  /*  Find  Target  Based  On  Tactical  Range  and  Bearing  */ 

if  (TRACE  ScSc  DISPLAYSCREEN) 

printf  {"[Sonar  searching  for  target] \n"); 
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previous_range  =  0.0; 

tiine_last_target_update  =  0.0; 


if 

{ 


} 


(TRACE  &&  (ST1000_range_kal  >  0.0)) 

printf  ( "ST1000_rangG_kal :  %5 . 11 f \nSonar  Bearing:  %5.11f\n", 

ST1000_range_kal,  normalize  (AUV_ST1000_bearing  +  psi)); 
printf  ("Target  Range:  %5 . Ilf XnTarget  Bearing:  %5.11f\n", 
target_range,  target_bearing) ; 


if  (ST1000_range_kal  >0.0) 

{ 

/*  Determine  location  of  return  in  world  coordinates  */ 
sonar_return_x  =  x  +  cos_psi  *  AUV_ST1000_x_of fset 
/*  X  position  of  sonar  */ 

-  sin_psi  *  AUV__ST1000_y_offset; 
sonar_return_:y  =  y  +  sinjsi  *  AUV_ST1000_x_of f set 
/*  y  position  of  sonar  */ 

+  cos_psi  *  AUV_ST1000_y_of fset; 
sonar_return_x  +=  cos  (radians  (psi  +  AUV_ST1000_bearing) ) 

*  ST1000_range_kal; 

sonar_return_y  +=  sin  (radians  (psi  +  AUV_ST1000_bearing) ) 

*  ST1000_range_kal; 

/*  Determine  location  of  vehicle  center  relative  to  target  */ 
new_target_bGaring  = 

normalize  (degrees  (atan2z  ( (sonar_return_y  -  y) , {sonar_rGturn_x  -  x)))); 
new_target_range  =  sqrt  (  (x  -  sonar_return_x) 

*  (X  -  sonar_return__x) 

+  (y  -  sonar_return_y ) 

*  (y  -  sonar_return_j/)  )  ; 

/*  Determine  if  Target  Located  */ 

if  ((fabs  (new_target_range  -  target_range)  <=  5.0)  && 

(fabs  {normalize2  (normalize  (new_target_bearing)  - 
target_bearing) )  <=  15.0)) 

{ 

i f  (TARGETEDGETRACK ) 

{ 

SONARSCANMODE  =2;  /*  use  target  edge  scan  */ 

/*  scan  towards  edge  on  side  move  is  towards  */ 
SONARSCAND I RECT ION 

dsign  (normalize2  (target_bearing  -  target_bearing_command) ) ; 
if  (TRUE  ScSc  DISPLAYSCREEN) 
if  (SONARSCANDIRECTION  ==  1) 

printf  ( "Sonar  Scanning  Right  To  Obtain  Edge\n" ) ; 
^  else  printf  ("Sonar  Scanning  LEFT  To  Obtain  Edge\n"); 

else 

{ 

SONARSCANMODE  =3;  /*  use  full  target  scan  */ 

previous_range  =  ST1000_range_kal  ; 

start_bearing  =  normalize  (psi+AUV_ST1000_bearing) ; 

no_return_count  =  0; 

scan_onto_target  =  FALSE; 

range_accumulator  =  ST1000_range_kal; 

■  valid_return_count  =  1; 

if  (TRUE  &&  DISPLAYSCREEN) 

printf  ("(Target  Located:  %5.11f  degrees,  %5.11f  feet]\n", 
normalize  (psi  +  AUV_ST1000_bearing)  ,  ST1000__rangG_kar)  ; 

} 

ping_sonar  (SONARSCANDIRECTION) ; 
break; 


case  (5):  /*  manual  (tactical  level)  scan  */ 


if  (TRACE  &&  DISPLAYSCREEN) 

{ 

printf  ("[Sonar  in  manual  scan  mode]\n"); 

printf  ("[AUV_ST1000_bearing  =  %5 . Ilf ] \n", AUV_ST1000_bearing) ; 
printf  ("[Command  ST1000_bearing  =  %5 . Ilf ] \n", stl000__command) ; 

} 

commanded_bearing_error  =  normal! ze2  (stl000_command-AUV_ST1000_bearing) 

if  (commanded_bearing_error  >  SONARHEADINGSTEP  /  2.0) 
SONARSCANDIRECTION  =  RIGHT; 

else  if  (commanded_bearing_error  <  -SONARHEADINGSTEP  /  2.0) 
SONARSCANDIRECTION  =  LEFT; 
else  SONARSCANDIRECTION  =  0; 
ping_sonar  (SONARSCANDIRECTION); 
break; 

default:  /*  Invalid  Mode  */ 

if  (DISPLAYSCREEN)  printf  ("[Invalid  sonar  scan  mode!  ***]\n"); 
break; 


if  (TRACE  Sc&  DISPLAYSCREEN) 

printf  ("[End  control_sonar ] \n" ) ; 


} 

/**********★*************★***************************************** *********^ 

void  open_virtual_world_socket  ()  /*  see  os9sender.c  for  original  code 

*/ 

{ 

if  (LOCATIONLAB  ==  FALSE)  /*  in  water  */ 

{ 

return; 

} 

if  (TRACE  ScSc  DISPLAYSCREEN) 

printf  ( " [ start  open_virtual_world_socket  ( ) ] \n" ) ; 

/*  Initialize  communications  blocks  */ 

/*  Initialize  both  client  &  server  ***********★*****************************/ 

/*  Signal  handlers  for  termination  to  override  net_open  ()  and  net_close 
()*/ 

/*  signal  handlers.  Otherwise  you  are  unable  to  ^C  kill  this  program. 

*/ 

#ifndef  os9 

signal  (SIGHUP,  shutdown_virtual_world_socket) ; /*  hangup  */ 

signal  (SIGINT,  shutdown_virtual_world_socket ) ; /*  interrupt  character 
*/ 

signal  (SIGKILL,  shutdown_.virtual_world_socket ) ; /*  kill  signal  from  Unix 
*/ 

signal  (SIGPIPE,  shutdown_virtual_world_socket) ; /*  broken  pipe  from  other 
host*/ 

signal  (SIGTERM,  shutdown_virtual_world_socket) ; /*  software  termination 
*/ 

#endif 

/*  Initialize  sender  *******************************************************/ 

/*  start  by  finding  default/desired  remote  host  to  connect  to  */ 

{ 


server_entity  =  gethostbyname  (virtual__world_remote_host_name) ; 
if  (server_entity  ==  NULL) 

{ 
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if  (DISPLAYSCREEN) 

{ 

printf ( " [open_virCual_world_socket :  virtual  world  remote  hostXn"); 
printf("  {\"%s\")  not  found] \n", 

virtual_world_reinote_host_name)  ; 
fflush  (stdout) ;  /*  force  completion  of  screen  write  */ 

} 

/*  error  message  needed  on  (open)  output  file  ««««««««<  */ 
virtual_world_socket_opened  =  FALSE; 
exit  (1); 

} 

else  if  {TRACE  &&  DISPLAYSCREEN) 

{ 

printf (" [open_virtual_world_socket :  virtual  world  remote  host  "); 
printf (" (\"%s\")  located] \n",  virtual_world_remote„host_name) ; 

} 

/*  Client  opens  server  port  ******************************************/ 

/*  Fill  in  structure  'server_address'  with  the  address  of  the  */ 

/*  remote  host  (i.e.  SERVER)  that  we  want  to  connect  with:  */ 

#ifdef  sgi 

bzero  ((char  *)  &:server_address,  sizeof  (server_address) )  ; 

#endif 

server_address.sin_family  =  AF_INET;  /*  Internet  protocol  family  */ 

/*  copy  server  IP  address  into  sockaddr_in  struct  server_address  */ 

#ifdef  sgi 

bcopy  ( server_enti ty->h_addr ,  & (server_address . sin_addr . s_addr) , 
server_entity->h_length) ; 

#else 

strncpy { (char  *)  Scserver_address . sin_addr. s_addr,  server_entity->h_addr, 
server__entity->h_length)  ; 

#endif 


/*  make  sure  port  is  in  network  byte  order  */ 

server_address.sin_port  =  htons  (AUVSIM1_TCP_P0RT_1) ; 

/*  Open  TCP  (Internet  stream)  socket  */ 

if  (  (socket_descriptor  =  socket  (AF_INET,  SOCK^STREAM,  0))  <  0  ) 

{ 

if  (DISPLAYSCREEN) 

{ 

printf  (" [open_virtual_world_socket :  client  can't  open  seirver"); 
printf  ("  virtual  world  stream  socket]"); 

} 

/*  error  message  needed  on  (open)  output  file  <<<<<<<<<<<<<<<<<<<  */ 
virtual_world_socket_opened  =  FALSE; 
exit  (1); 

} 

else  if  (TRACE  &&  DISPLAYSCREEN) 

{ 

printf  (" [open_virtual_world_socket:  client  opened"); 
printf  ("  virtual  world  server  socket  successfully] \n") ; 

} 

/*  Connect  to  the  server.  Process  will  block/sleep  until  connection  is 
is  established.  Timeout  will  return  an  error.  */ 

if  (connect  (  socket^descriptor, 

(struct  sockaddr  *)  £cserver_address, 

sizeof  (server_address) )  <  0) 

{ 

if  (DISPLAYSCREEN) 

{ 

printf  ( " [open_virtual_world_socket :  client  can't  connect  to" ) ; 
printf  {"  virtual  world  server  socket] \n"); 

} 

/*  error  message  needed  on  (open)  output  file  <<<<<<<<<<<<<<<<<<<  */ 
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virtual_world_socket_opened  =  FALSE; 
exit  (1); 

} 

else  if  (TRACE  &&  DISPLAYSCREEN) 

{ 

printf  {"[execution  client  connected  to  virtual  world"); 
printf  ("  server  socket  successfully] \n") ; 

} 

virtual__world_socket_opened  =  TRUE; 

}  /*  end  initialization  */ 


socket_streaiii  =  socket_descriptor;  /*  client  */ 


if  (TRACE  &&  DISPLAYSCREEN)  /*  print  final  info  */ 

{ 


printf (" [open_virtual_world_socket  CLIENT: 
printf (" ( 
printf (" [ 


} 


socket_descriptor  = 
socket_descriptor) ; 
socket_accepted  = 
socket_accepted) ; 
socket__streain  = 
socket_stream)  ; 


%d]\n", 

%d]\n", 

%d]\n". 


if  (TRACE  ScSc  DISPLAYSCREEN) 

printf  ("[finish  open_virtual_world_socket  ()]\n"); 

return; 

}/*  end  open_virtual_world_socket  (}  */ 

/****** *************************************ifr*******************************y 

void  shutdown_virtual_world_socket  ()  /*  see  os9sender.c  for  original  code 

*/ 

{ 

int  kill_return_value; 

shutdown_signal_received  =  TRUE; 

if  (LOCATIONLAB  ==  FALSE)  /*  in  water  */ 

{ 

return; 

} 

if  (virtual_world_socket_opened  ==  FALSE) 

{ 

if  (TRACE  &&  DISPLAYSCREEN) 

{ 

printf  ("  Ivirtual_world__socket_opened  FALSE,  ")  ; 
printf  ("  shutdown_virtual_world_socket  ignored] \n" ) ; 

} 

return; 

} 

if  (TRACE  &&  DISPLAYSCREEN) 

printf  (" [shutdown_virtual_world_socket  start  . . . ] \n") ; 

/*  No  need  to  send  a  message  to  other  side  that  bridge  is  going  down,  */ 

/*  since  SIGPIPR  signal  trigger  may  shutdown  server  on  other  side  */ 

if  (close  (socket^stream)  ==  -1) 

{ 

if  (TRACE  ScSc  DISPLAYSCREEN) 

printf  ("shutdown_virtual__world_socket  close  (socket_stream) 
failedXn" ) ; 

/*  shutdown  ()  reference:  "Using  OS-9  Internet"  manual  p.  2-55  */ 
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-1) 


if  (shutdown  (socket_stream,  2)  == 

{ 

if  (TRACE  ScSc  DISPLAYSCREEN) 

{ 

printf  ( "  lshutdown__virtual_world_socket  shutdown"); 
printf  ("  (socket_stream,  2)  failed] \n"); 

} 

kill_return_value  =  kill  (socket_stream,  SIGKILL) ; 

if  (TRACE  ScSc  DISPLAYSCREEN) 

{ 

printf  (" [shutdown_yirtual_world_soc]cet  kill  (socket_stream, " ) ; 
printf  ("  SIGKILL)  returned  %d]\n",  kill_return_value) ; 

} 

} 

} 

if  (TRACE  ScSc  DISPLAYSCREEN) 

printf  ( " [ shutdown_virtual_world_socket  return] \n" ) ; 

return; 

}  /*  end  shutdown_virtual_world_socket  ()  */ 

/****************  **********★**************»:*******★*********************  *****/ 

void  send_buf fer_to_virtual_world_socket  {)  /*  see  osPsender.c  for  orig.  code 
*/ 

{ 

bytes_left  =  socket_length; 

bytes_written  =  0; 

ptr_index  =  buffer;  /*  this  global  string  is  the  data  to  be  sent  */ 

if  (LOCATIONLAB  ==  FALSE)  /*  in  water  */ 

{ 

return; 

} 

if  (virtual_world_socket_opened  ==  FALSE) 

{ 

if  (TRACE  ScSc  DISPLAYSCREEN) 

{ 

printf  ( " [ send_buf f er_to_virtual_world_socket :  " ) ; 

printf  ("virtual_world_socket_opened  ==  FALSE,  returning] \n" ) ; 

} 

return; 

} 

if  (TRACE  ScSc  DISPLAYSCREEN) 

printf  (" [send_buffer_to_virtual_world_socket  start  . . . ] \n") ; 

while  (  (bytes_lef t  >  0)  ScSc  (bytes_written  >=  0))  /*  write  loop  ***********/ 

{ 

bytes_sent  =  write  (socket^stream,  ptr_index,  bytes_left); 

if  (bytes_sent  <  0)  bytes__written  =  bytes_sent; 

else  if  (bytes_sent  >  0) 

{ 

bytes__left  bytes_sent; 

bytes_written  +=  bytes_sent; 
ptr_index  +=  bytes_sent; 

} 

if  (LOCATIONLAB  &&  TRACE  &&  DISPLAYSCREEN) 

{ 

printf  ( "  [record_data  send_teleinetry__t  observer  loop"); 
printf  ("  bytes  sent  =  %d]\n",  bytes_sent) ; 

} 

} 

if  (bytes_written  <  0) 
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{ 

if  (LOCATIONLAB  &&  DISPLAYSCREEN) 

{ 

printf  ( " [record_data  send_teleinetry_to„server  ()  send  failed,  "); 
printf  ( "%d  bytes_written] \n",  bytes_written) ; 

} 

/*  error  message  needed  on  (open)  output  file  <<<<<<<<<<<<<<<<<<<  */ 

} 

else  if  (LOCATIONLAB  &&  TRACE  &&  DISPLAYSCREEN) 

{ 

printf  ("  [record_data  send_teleinetry_t  observer  total  bytes  sent"); 
printf  {"  =  %d]\n",  bytes_written) ; 

} 


/*  Check  termination  ************************★***************************/ 

if  (strncmp  (buffer,  "shutdown",  8)  ==  0) 

{ 

if  (TRACE  ScSc  DISPLAYSCREEN)  printf 

( " [send_buf fer_to_virtual_world__socket :  shutdown's ignal_received] \n" ) ; 
shutdown_virtual_world_socket  ( ) ; 

) 

if  (TRACE  ScSc  DISPLAYSCREEN) 

printf  (" [send_buf fer_to_virtual_world_socket  return] \n") ; 
return; 

}  /*  end  send_buf fer__to_virtual_world_socket  ()  */ 
/*******************************************************************★*******/ 


void  get__string_from_virtual_world_socket  {) 
*/ 


/*  see  os9sender.c  for  original 


{ 

if  (LOCATIONLAB  ==  FALSE)  /*  in  water  */ 

{ 

return; 

} 

if  {virtual_world_socket_opened  ==  FALSE) 

{ 

return; 

} 

if  (TRACE  ScSc  DISPLAYSCREEN) 

printf  (" lget_string_f rom_virtual_world_socket  start  ...]\n"); 

/*  listen  to  remote  host,  relay  to  local  network/program  */ 

bytes_left  =  socket_length; 

'  bytes_received  =  0; 

ptr_index  =  buf fer_received;  /*  buf fer_received  is  where  results 

go*/ 


while  ( (bytes_left  >  0)  &&  (bytes_received  >=  0))  /*  read  loop  *********/ 

{ 

bytes__read  =  read  (socket_stream,  ptr_index,  bytes__lef t)  ; 

if  (bytes_read  <  0)  bytes__received  =  bytes^read; 

else  if  (bytes_read  >  0) 

{ 

bytes_left  -=  bytes_read; 
bytes^received  +=  bytes_read; 
ptr_index  +=  bytes_read; 

} 

if  (TRACE  &&  DISPLAYSCREEN) 

{ 

printf  (" [get_string_from_virtual_world_socket  receiver  block"); 
printf  ("  loop  bytes_read  =  %d]\n",  by t e spread); 
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} 

/*  if  nothing  is  waiting  to  be  read,  break  out  of  read  loop  */ 

if  ( {bytes_read  ==  0)  &&  (bytes_received  ==  0))  break; 

} 

if  (bytes_received  <  0)  /*  failure  */ 

{ 

if  (TRACE  ScSc  DISPLAYSCREEN) 

{ 

printf  {" [get_string_froni_virtual_world_socket  receiver  block  read"); 
printf  ("  failed,  bytes__received  =  %d\n",  bytes_received) ; 

} 

else  if  (bytes_received  ==  0)  /*  no  transfer  */ 

{ 

if  (TRACE  ScSc  DISPLAYSCREEN) 

{ 

printf (" [get_string_from_virtual_world_socket  received  0  bytesl ! ] \n") ; 

} 

else  if  (bytes_received  >0)  /*  success  */ 

{ 

if  (TRACE  &&  DISPLAYSCREEN) 

{ 

printf (" [get_string„from_virtual_world_socket  received  %d  bytes]\n", 
bytes_received) ; 

} 

} 

/*  Check  termination  ****************************************************/ 

if  (strncmp  (buf fer_received,  "shutdown",  8)  ==  0) 

{ 

if  (TRACE  &&  DISPLAYSCREEN)  printf 

{" [get_data_on_virtual_world_socket :  shutdown's ignal_receivedj \n" ) ; 
shutdown_virtual_world_socket  () ; 

} 

if  (TRACE  Sc&  DISPLAYSCREEN) 

printf  ( " [get__string_f rom_virtual_world_socket  return] \n" ) ; 
return; 

}  /*  end  get_string_f rom_virtual_world_socket  ()  */ 

/**************************************************************** ***********y 

void  record_data  ()  /*  this  needs  to  be  better  partitioned  «««««««  */ 


static  int  count  =  50; 

if  (TRACE  StSc  DISPLAYSCREEN)  printf  ("[start  record_data  ()]\n"); 

system_time  =  time  (NULL) ; 

system_tmp  =  localtime  (Scsystem_time)  ; 

if  (TRACE  ScSc  DISPLAYSCREEN) 

printf  ("[OS-9  system  time  is  %02d: %02d: %02d,  replication  %d]\n", 

system_tmp->tm_hour,  system_tmp->tm_min,  system_tmp->tm_sec , 
replication_count ) ; 

build_telemetry_string  (buffer); 

if  (TACT  I  CAL  PARSE  ==  FALSE)  /*  not  used  by  tactical  level  */ 

send_buffer_to__virtual_world_sockGt  ();  /*  there  it  goes  */ 

if  (TRACE  ScSc  DISPLAYSCREEN)  /*  telemetry  report  to  screen  */ 

printf  ("\nsending  to  virtual  world:"); 
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printf  ("\n%s",  buffer); 

} 

else  if  (DISPLAYSCREEN)  /*  partial  telemetry  report 

{ 

if  (count  ==  50) 

{ 

printf("[t  =  %5.1f,  voltages:  %5.1f  %5.1f]\n", 

t ,  computer_voltage,motor__voltage)  ; 
count  =  0; 

} 

++count ; 


if 

{ 


} 


(LOCATIONLAB  &&  CiSPLAYSCREEN) 

printf  ("sent  telemetry  to  virtual  world  %5.21f  ",  t) ; 
printf  ("(%5.11f  %5.11f  %5.11f  %5.11f  %5.11f  %5.11f)\n", 
x,y, z,  phi, theta, psi) ; 


*/ 


if  (LOCATIONLAB) 

{ 

get_string_from_virtual_world_socket  ();  /*  here  it  comes  */ 

if  (REPLAY) 

{ 

read_telemetry_string  ()  ; 

bu i  1  d_tel erne try_st ring  (buffer) ; 

bu i ld_tel erne try_st ring  (buf  fer_received)  ; 

} 

else  parse_telemetry_string  (buf  fer_received)  ; 


if  (TRACE  ScSc  LOCATIONLAB  &&  DISPLAYSCREEN) 

{ 

printf  (" - \n"); 

} 

if  (  ((TACTICAL  ==  FALSE)  I!  (TACTICALPARSE) )  &&  (auvdataf ile  ! =  NULL) 

ScSc  (NOSCRIPT  ==  FALSE)) 

/*  output  data  to  telemetry  file  */ 

{ 

/*  note  that  unmodified  stream  is  saved  */ 
if  (buffer_size  ==  0)  /*  nothing  was  received,  send  auv_state  */ 

fprintf  (auvdataf ile,  "%s",  buffer); 

else  /*  feedback  was  received,  send  uvw_state  */ 

fprintf  (auvdataf ile,  "%s",  buf fer_received) ; 

if  (TRACE  ScSc  DISPLAYSCREEN) 

printf (" [printed  to  %s  telemetry  file]\n",  AUVDATAF ILENAME ) ; 

} 

/*  only  send/print  out  every  10th  telemetry  entry  to  tactical  level  */ 

/*  due  to  serial  port  bandwidth  limitations  :-(  */ 

if  ((TACTICAL)  ScSc  TRACE  ScSc  DISPLAYSCREEN) 

printf  ("[sending  data  to  tactical  level] \n"); 

#ifndef  os9 

/*  --  NOT  YET  UPDATED  TO  CURRENT/OPERATIONAL  MARCO  AUV  HARDWARE /SOFTWARE  --  */ 

/*  writeln  (serialpath,  buffer,  buf fer_max) ;  ««<  */ 
/*  if  (TACTICAL)  write  (serialpath,  buffer,  buffer_max) ; 

if  ((TACTICAL)  ScSc  TRACE  ScSc  DISPLAYSCREEN) 

printf  ("[write  buffer  to  tactical  level  serialpath  OK]\n");  */ 

#endif 
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if  (TACTICAL)  send_buf fer__to_tactical_socket:  {)  ;  /*  telemetry  */ 

if  (auvtextfile  !=  NULL) 

/*  output  data  to  .auv  text  file  */ 

{ 

if  (TRACE  &&  DISPLAYSCREEN) 

printf  ("[sending  data  to  .auv  text  file]\n"); 

fprintf  (auvtextfile,  "%s",  buffer); 

if  (buffer_size  !=  0)  /^feedback  was  received,  also  send  uvw__state  */ 
fprintf  (auvtextfile,  "%s",  buff er_received) ; 

if  (TRACE  &&  DISPLAYSCREEN) 

printf  ("[fprintf  to  .auv  text  file  OK]\n"); 

} 

telemetry_records_saved  ++; 

if  ( ( (buffer_index  +1)  %  FILEBUFFERSIZE)  ==  0)  buf fer__index  =  0; 
else  buffer_index  ++; 


/*  need  to  copy  buffer  to  buffer_array  if  caching  telemetry  «««««<  */ 
if  (TRACE  ScSc  DISPLAYSCREEN)  printf  ( "  [buf f er^index  =  %d]\n",  buffer^index); 

/*  test  code  to  send  data  from  file  serial. d  from  wrZtl.c  */ 
/*  read  characters  from  file,  echo  characters  to  screen,  .  */ 
/*  send  characters  to  serialpath  /tl  device  */ 


/*  while  ((c[0]  =  getc (serialtestf ile) )  1=  EOF) 

{ 

putc  (c [0] , stdout ) ; 
writeln  (serialpath,  c,  1); 

} 

*/ 

/* - 


if  (TRACE  &&  DISPLAYSCREEN)  printf  ("[finish  record_data  ()]\n"); 
return; 


void  execute_shutdown_script ( ) 

{ 

static  int  phase  =  1; 
static  double  time_next_phase; 

if  (DISPLAYSCREEN  &&  (phase  ==  1))  printf (" [Shu tdown  Script  Entered] \n" ) ; 

switch  (phase) 

{ 

case  1: 

if  (TRUE  ScSc  DISPLAYSCREEN)  printf  ("  [Starting  Phase  1  of  Shutdown  Script]\n"); 
THRUSTERCONTROL  =  TRUE; 

ROTATECONTROL  =  FALSE; 

LATERALCONTROL  =  FALSE; 

FOLLOWWAYPOINTMODE  =  FALSE; 

WAYPOINTCONTROL  =  FALSE; 

HOVERCONTROL  =  FALSE; 

GPSFIXINPROGRESS  =  FALSE; 
x^command  =  x; 
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y^command  =  y; 
z^command  =  -10; 
psi_command  =  psi; 
rpm  =  0.0; 

port_rpin_command  =  0.0; 
stbd_rpm_coinrnand  =  0.0; 
tiine_next_phase  =  t  +  20.0; 
phase  =  2; 
break; 
case  2 : 

if  (t  >=  tiine_next_phase) 

{ 

if  (TRUE  ScSc  DISPLAYSCREEN)  printf  ("Starting  Phase  2  of  Shutdown  ScriptXn"); 
THRUSTERCONTROL  =  FALSE; 
tiine_next_phase  =  t  +  1,0; 

phase  =  3; 

} 

break; 
case  3: 

if  (t  >=  tiine_next_phase) 

{ 


if  (TRUE  ScSc  DISPLAYSCREEN)  printf  ("Starting  Phase  3  of  Shutdown  Script\n"); 
LOOPFOREVER  =  FALSE; 
strcpy  (buffer,  "KILL"); 

send_buf fer„to_virtual_world_socket  ();  /*  buffer  insg  sent  */ 
if  (TRACE  &Sc  DISPLAYSCREEN)  printf  ( "\n [end_test  set  TRUE]\n"); 
end_ t  G  St  =  TRUE ; 


if  (NOSCRIPT  ==  FALSE)  fclose  (auvscriptf ile) ; 
auvscriptf ilequit  =  TRUE; 
if  (DISPLAYSCREEN) 

printf ("\n[QUIT  condition: (%s  backup  file)  mission. script .backup, 
file  closed] \n" , 


AUVSCRIPTFILENAME) ; 


x_dot 

= 

0.0; 

y_dot 

0.0; 

= 

0.0; 

phi_dot 

= 

0.0;  /* 

degrees/sec 

*/ 

theta_dot 

= 

0.0;  /* 

degrees /sec 

*/ 

psi_dot 

= 

0.0;  /* 

degrees/sec 

*/ 

speed 

= 

0.0; 

u 

= 

0.0; 

V 

0.0; 

w 

0.0; 

p 

= 

0.0;  /* 

degrees/sec 

*/ 

q 

= 

0.0;  /* 

degrees/sec 

*/ 

r 

= 

0.0;  /* 

degrees /sec 

*/ 

dGlta_planGs 

= 

0.0;  /* 

degrees 

*/ 

delta^rudder 

= 

0.0;  /* 

degrees 

*/ 

port_rpm 

= 

0; 

stbd_rpm 

=: 

0; 

vertical_thrustGr__volts 

0.0; 

latGral_thruster_volts 

= 

0.0; 

phase  =  3; 


} 

break; 
default : 
break; 


} 

/****************************************************★**********************/ 
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double  read__coinputer_batterY__voltage  () 

{ 

int  val ; 
double  voltage; 

val  =  get^adcl  (COMPUTER_VOLTAGE_CH) ; 

/*  3.03  Since  a  voltage  divider  Circuit  by  Approx  3  */ 
voltage  =  (10.0/512.0)  *  ((double)  val  -  512.0)  *  3.03; 

if  (TRACE  ScSc  DISPLAYSCREEN) 

printf  ("read_computer_battery_voltage  () :  val  =  %d\n",  val); 
if  (TRACE  &&  DISPLAYSCREEN) 

printf  ("read_coinputer_battery_voltage  {):  voltage  =  %f\n'', 
voltage) ; 


return  (voltage) ; 

}  /*  end  read_coinputer_battery_voltage  ()  */ 

/*******************★*★***************************************************** y 

double  read_motor_gyro_battery_voltage  () 

{ 

int  val; 
double  voltage; 

val  =  get_adcl  (MOTOR_GYRO_VOLTAGE_CH) ; 

/*  3.03  Since  a  voltage  divider  Circuit  by  Approx  3  */ 
voltage  =  (10.0/512.0)  *  ((double)  val  -  512.0)  *  3.03; 

if  (TRACE  ScSc  DISPLAYSCREEN) 

printf  ("read_inotor_gyro_battery_voltage  ()  :  val  =  %d\n",val); 
if  (TRACE  ScSc  DISPLAYSCREEN) 

printf  ("read_motor_gyro_battery_voltage  () :  voltage  =  %f\n", 
voltage) ; 


return  (voltage) ; 

}  /*  end  read_motor__gyro_battery_voltage  ()  */ 


int  leak_check  () 

{ 

int  1/  adc_value  [8],  bow_adc_value,  stern_adc_value; 
double  bow_voltage,  stern__voltage; 

LEAK  =  FALSE; 

for  (i=0;i<8;++i) 

{ 

adc_yalue  [i]  =  get_adcl(i); 

} 

bow_adc_value  =  adc_value [5] ; 

bow_voltage  =  (10 . 0/512 . 0)  *  (bow_adc_value  *-  512.0); 
if  (TRACE  ScSc  DISPLAYSCREEN) 

printf  ("leak_check  ():  bow_voltage  =  %5 . If \n" ,bow_voltage) ; 


if  (bow__voltage  >  1.7) 

{ 

if  (DISPLAYSCREEN) 

printf  (''*****  BOW  LEAK  DETECTED  *****  bow_voltage  =  %f\n",  bow_voltage)  ; 
LEAK  =  TRUE; 
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} 


stern_adc_value  =  adc^value  [6]; 
if  (TRACE  DISPLAYSCREEN) 

printf  ("stern_adc_value  =  %d\n'' ,  st:ern_adc_value)  ; 

stern_voltage  =  (10 . 0/512 . 0)  *  (st:ern_adc_value  ~  512.0); 

if  (TRACE  ScSc  DISPLAYSCREEN) 

printf  ("leak_check  {):  stern__voltage  =  %5.1f\n",  stern_voltage) ; 
if  (stern_voltage  >4.0)  /*  avoid  spurious  detections  (originally  1.7)  */ 
if  (DISPLAYSCREEN) 

printf  (''*****  STERN  LEAK  DETECTED  *****  stern_voltage  =  %f\n", 
stern_voltage)  ; 

LEAK  =  TRUE; 

) 

return  (LEAK); 

}  /*  end  leak_check  {)  */ 

/ic***i:*  ********************************************  **ic**ic*i,i,icicic***icic*******ic/ 

/*  Dive  Tracker  Functions  Won't  Compile  on  SGI  */ 

#ifdef  os9 

int  createdmodO 
{ 

mod_data  *dat_struct; 

/*  Create  to  data  module  */ 

dt_dinod=CreateMod("DT2CL" ,sizeof (DT2CLMem) , &dat_s truct ) ; 
if(dt_dmod  ==  NULL)  { 

if  (DISPLAYSCREEN)  printf ("Data  Module  exists.  Not  createdXn"); 
exi t ( 1 ) ; 

} 

d  t_dmod  -  >da  t  a_s  t  a  tu  s = CL_R  ; 


} 

int  CLReaddmod{rl_ptr,r2jtr) 
int  *rl_ptr; 
int  *r2_ptr; 

{ 


static  int  rl,r2; 


if  {dt_dmod->data_status==DT_W) 
{ 

rl=dt_dmod->dtr . rl ; 
r2=dt_dmod->dtr . r2; 
d  t_dmod“  >da  ta_s  t  a  tu  s= CL_R ; 
*rl__ptr=rl; 

*r2_ptr=:r2; 
return  (NEW_DATA)  ; 

} 

/ 


***********************  ************************i:icic*****ic********************^ 

/*************************^^^^gQ  code  ************************************/ 
/***************  Dave  Mcclairen  needs  a  negative  value  to  signal  that  there  is 
no  new  data  recorded  by  the  dive  tracker.  ******************/ 


/*  *rl_ptr=*-l; /*change  in  code  from  rl  to  -1  to  signal  no  update  to  tactical*/ 
/*  *r2_ptr=-l; /*change  in  code  from  r2  to  -1  to  signal  no  update  to  tactical*/ 
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r  e  t u  r n  ( OLD_DATA )  ; 


void  *AttachMod{str,data_struct) 
char  *str; 

inod_dat:a  **data_struct; 

{ 

/*  Trying  to  link  to  the  Data  Module  */ 

if  {  (*data_struct=modlink(str,ANY)  )=={inod_data  *)-!  )  { 
return (NULL) ; 

} 


/*  ...  then  prepares  an  auxiliar  structure  to  point  to  the  Data  Module  */ 
return{  (void  * ) ( (long) ( (mod^data  * ) *data_struct) 

+(long) (*data_struct)->data_offset)  ); 


int  DettachMod (data_struct ) 
mod__data  *data_struct ; 

{ 


if  (  inunlink{data_struct)  ==  (niod_data  *)-l  )  { 
return (-1 ) ; 

} 

return (0) ; 


void  *CreateMod  (str,  size,  data__s true t) 
char  *str; 
unsigned  size; 
mod^data  **data_struct; 

{ 

/*  Creates  Data  Module  */ 

if(  (*data_struct=_inkdata_module 

(str,size,  mka tt revs {MA_RE ENT  I  MA_GHOST, 0x00) , Perm_fi eld) )  == 
(mod_data  *)-! 

)  { 

return  (NULL)  ; 

} 


/ 

} 


*  . . .  then  prepares  an  auxiliar  structure  to  point  to  the  Data  Module 
return(  (void  * )  (  (long)  (  (modjata  *)  *data_struct) 

+(long) ( *data_struct) ~>data_of fset )  ); 


*/ 


#endif 

/********************************************************** *********^***^^^^^ 

/*  Mathematical  Model  for  Estimating  X  and  Y  */ 

void  XY_model__est  { v_ls ,  v_rs ,  v_bl t ,  v_sl t ,  X_dot_c ,  Y_dot_c ,  update_vel ) 

double  v_l  s ,  v_rs ,  v_bl t ,  v_s  1 1 ,  X_dot_c ,  Y_dot_c ; 

unsigned  short  update_vel; 

{ 

double  alpa__x  =  0.0025, 
alpa_y  =  0.004, 
b_x  =  1.33, 

b^  =  17.0; 

double  M_x  =  {435.0  +  43.5)  /  32.2, 

M_y  =  (435.0  +  348.0)  /  32.2; 

double  f_ls,  f_rs,  f_blt,  f_slt,F_x,F_Y; 

dou  b  1  e  u_ddo  t ,  v__ddo  t ,  r_ddo  t  ; 
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f_ls  =  alpa_x*  {v_ls*v__ls)  *dsign  (v_ls)  ; 
f_rs  =  alpa_x*  (v_rs*v_rs)  *dsign(v__rs)  ; 

f_blt  =  alpa_^*  (v_blt*v__blt)  *dsign  (v_blt )  ; 
f_slt  =  alpa_y* (v_slt*v_slt} *dsign(v_slt) ; 

F_x  =  f_ls  +  f_rs; 

F_y  =  f_blt  + 


/*  Use  speed  sensor  OR  mathematical  model  to  estimate  speed  */ 
/*  depending  on  previous  speed  and  speed  sensor  value  */ 

u_ddot  =  (F_x  -  b_x*u*fabs (u) ) /M_x; 
if  ({u  >  0.2)  ScSc  (speed  >=  0.2)) 

{ 

u  =  speed; 

} 

else  if  (  (u  <  -0.2)  &&  (speed  >=  0,2)) 

{ 

speed  =  -speed; 
u  =  speed; 

} 

else 

{ 

u  =  u  +  dt  *  (u_ddot) ; 

if  (u  >  0 .24)  u  =  0 .24; 
else  if  (u  <  -0.24)  u  =  -0,24; 

speed  =  u; 

} 

v_ddot  =  (F^  -  b_:y*v*fabs  (V)  ) /M_/; 

V  -  V  +  dt*  (v__ddot)  ; 

if ( !update_vel) 

{ 

u  =  x_dot*cos_i)si  +  y_dot*sin_psi; 

V  =  -x_dot*sin_j)si  +  y_dot*cos__psi ; 


/*  modify  state  vector  values  based  on  dead  reckoning  */ 

x_dot  =  u*cos_psi  -  v*sin_psi  +  X_dot__c; 
y_dot  =  u*sin_psi  +  v*cos_psi  +  Y_dot_c; 

X  +=  dt  *  x_dot; 
y  +=  dt  *  y_d6t; 

return; 


/*  Constant  gain  Kalman  filter  for  depth  */ 
void  kalman_z (yk) 

double  yk; 

{ 

double  xkl__0,xkl_l,xkl_2  ; 

double  phii[3] [3],h[3] ,b[3],lk[31,res; 

/*  a=[0  1  0;0  0  1;0  0  0];  phii=expm (a*0 . 1 ) /  where  dt  =  0.1  */ 
b[0]  =  0.0; 
b[l]  =  0.0; 
b[2]  =  1.0; 
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/*  phii  =  [1.0  0.1  0.005 

0.0  1.0  0.1 

0.0  0.0  1.0]  */ 

phii [0] [0]  =  1.0; 
phii [0] [1]  =  0.1; 
phii [0] [2]  =  0.005; 
phii [1] [0]  =  0.0; 
phii[l] [1]  =  1.0; 
phii [1] [2]  =  0.1; 
phii [2] [0]  =  0.0; 
phii [2] [1]  =  0.0; 
phii [2] [2]  =  1.0; 

/*  h  =  (10  0]  ;  */ 
h[0]  =  1.0; 
h[l]  =  0.0; 
h[2]  =  0.0; 

if {kal_init_2  ==  1) 

{ 

z_kal  =  yk; 
z_dot_kal  =  0.0; 

2_ddot_kal  =  0.0; 

/*  xkl=xk;  */ 
xkl_0  =  z_kal; 
xkl_l  =  z_dot_kal; 
xkl_2  =  z_ddot_kal; 
kal_init_z  =  FALSE; 

} 

/*  set  Ik  =  const.  Slow  Filter  */ 
lk[0]  =  0.2544; 
lk[l]  =  0.3727; 
lk[2]  =  0.2731; 

/*  xkl ( : ,i)=phii*xk( : , i) ;  */ 

xkl_0  =  phii  [0]  [0]  *z__kal  +  phii  [0]  [1]  *2_dot__kal  +  phii  [0]  [2]  *z_ddot_kal; 

xkl_l  =  phii  [1  ]  [0]  *z_kal  +  phii  [1]  [1]  *z_dot__kal  +  phii  [1]  [2]  *z__ddot_kal; 

xkl_2  =  phii [2] [0] *2_kal  +  phii [2 ] [1 ] *z_dot_kal  +  phii [2] [2] *z_ddot_kal; 

res  =  yk  -  (h[0]*xkl_0  +  h[l]*xkl_l  +  h[2]*xkl_2); 

/*  Set  res  =  0.0  if  larger  than  threshold  */ 
if(fabs(res}  >  thres_z) 

{ 

res  =  0.0; 

} 

2_kal  =  xkl_0  +  lk[0]*res; 
z_dot_kal  =  xkl_l  +  lk[l]*res; 

2_ddot_kal  =  xkl_2  +  lk[2]*res; 

return; 


/**★*★********************************************************** 

/*  Constant  gain  Kalman  filter  for  STIOOO  range  */ 
void  kalman_sonarl000  (yk) 

double  yk; 

{ 

double  xkl_0,xkl_l,xkl_2; 

double  phii [3] [3] ,h[3] ,b [3 ] , Ik [3 ] , res; 

/*  a=[0  1  0;0  0  1;0  0  0];  phii=expm ( a*0 . 1 ) ;  where  dt  =  0.1  */ 
b[0]  =  0.0; 
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b[l]  =  0.0; 
b[2]  =  1.0; 


/*  phii  = 


phii[0] [0] 
phii[0] [1] 
phii[0] [2] 
phii [1] [0] 
phiid]  [1] 
phii[l] [2] 
phii [2] [0] 
phii[2] [1] 
phii [2] [2] 


[1.0  0.1 

0.0  1.0 

0.0  0.0 

=  1.0; 

=  0.1; 

=  0.005; 

=  0.0; 

=  1.0; 

=  0.1; 

=  0.0; 

=  0.0; 

=  1.0; 


0.005 

0.1 

1.0]  */ 


/*  h  =  [10  0]  ;  */ 
h[0]  =  1.0; 
h[l]  =  0.0; 
h[2]  =  0.0; 


if (reset_sonar_filter) 

{ 

ST1000__range_kal  =  yk; 
ST1000__range_kal_dot  =  0.0; 
ST1000_range_kal_ddot  =  0.0; 

/*  xkl=xk;  */ 
xkl_0  =  ST1000_range_kal; 
xkl_l  =  ST1000_range_kal_dot; 
xkl_2  =  ST1000_range_kal_ddot; 
reset_.sonar_filter  =  FALSE; 


/*  set  Ik  =  const.  Slow  Filter  */ 
lk[0]  =  0.2544; 
lk[l]  =  0.3727; 
lk[2]  =  0.2731; 


/*  xkl ( : , i) =phii*xk{ : , i) ;  */ 

xkl_0  =  phii[0]  [0]*ST1000__range_kal 

+  phii [0] [1] *ST1000_range_kal_dot 
+  phii[0] [2 ] *ST1000_range_kal_ddot; 
xkl_l  =  phii[l] [0]*ST1000_range_kal 

+  phi i [ 1 ] [ 1 ] *ST1 0  0  0_range_kal_do t 
+  phii[l] [2] *ST1000_range_kal_ddot; 
xkl_2  =  phii[2] [0]*ST1000_range_kal 

+  phii [2] [1] *ST1000_range_kal_dot 
+  phii [2] [2] *ST1000_range_kal_ddot; 

res  =  yk  -  {h[0]*xkl__0  +  h[l]*xkl_l  +  h[2]*xkl_2}; 

/*  Set  res  =  0.0  if  larger  than  threshold  */ 
if (fabs (res)  >  5.0) 

{ 

res  =  0.0; 

} 


ST1000_range_kal 
ST10  00_range_kal__dot  = 
ST1000_range_kal_ddot  = 


xkl_0  +  lk[0]*res; 
xkl_l  +  lk[l]*res; 
xkl_2  +  lk[2]*res; 


/*****************-k***-kic*ic-kic*i:-k*ic***i(***ic*ic*’kicicic*ic****i(*-k*ick-k-k-k-k**-k*-k*icic-k-k*-k^ 

/*  Constant  gain  Kalman  filter  for  ST725  range  */ 
void  kalman_sonar725 (yk) 
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double  yk; 

{ 

double  xkl_0 , xkl_l , xkl_2 ; 

double  Phil [3] [3 ] , h [3] ,b [3 ] , Ik [3 ] , res; 

/*  a=(0  1  0;0  0  1;0  0  0];  phii=expin (a*0 . 1 ) ;  where  dt  =  0.1  */ 
b[0]  =  0.0; 
b[li  =  0.0; 
b[2]  =  1.0; 


/*  phii  =  [1.0 

0.1 

0.005 

0.0 

1.0 

0.1 

0.0 

0.0 

1.0]  */ 

phii [0] [0]  =  1.0; 
phii  [0]  [1]  =  0.1; 


phii [0] [2]  =  0.005; 
phiid)  [0]  =  0.0; 
phii[l] [1]  =  1.0; 
phii[l] [2]  =  0.1; 
phii  [2] [0]  =  0.0; 
phii  [2] [1]  =  0.0; 
phii  [2] [2]  =  1.0; 

/*  h  =  [100] ;  */ 
h[0]  =  1.0; 
h[l]  =  0.0; 
h[2]  =  0.0; 

if (NEWRECOVERYCOMMAND) 

{ 

ST725_range_kal  =  yk; 
ST725_range_kal_dot:  =  0.0; 
ST725_range_kal_ddot  =  0.0; 

/*  xkl=xk;  */ 
xkl_0  =  ST725_range_kal ; 
xkl_l  =  ST725_range_kaJ_dot; 
xkl_2  =  ST725_range_kal_ddot; 


/*  set  Ik  =  const.  Slow  Filter  */ 
lk[0]  =  0.2544; 
lk[l}  =  0.3727; 
lk[2]  =  0.2731; 

/*  xkl ( : ,i)=phii*xk( : ,  i) ;  */ 
xkl_0  =  phii [0] [0] *ST725_range_kal 

+  phii [0] [1] *ST725_range_kal_dot 
+  phii[0] [2] *ST725_range_kal_ddot; 
xkl_l  =  phii [1] [0] *ST725_range_kal 

+  phii[l] [l]*ST725_range_kal_dot 
+  phii[l] [2] *ST725_range_kal_ddot; 
xkl_2  =  phii[2] [0]*ST725_range_kal 

+  phii [2 ] [1] *ST725_range_kal_dot 
+  phii [2] [2]*ST725_range_kal_ddot; 

res  =  yk  -  (hI0]*xkl_0  +  h[l]*xkl_l  +  h[2]*xkl_2); 

/*  Set  res  =  0.0  if  larger  than  threshold  */ 
if(fabs(res)  >  thres_z) 

{ 

res  =  0.0; 

} 

ST725_range_kal  =  xkl_0  +  lk[0]*res; 

ST725_range_kal_dot  =  xkl_l  +  lk[l}*res; 
ST725_range_kal_ddot  =  xkl_2  +  lk[2]*res; 
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/******************-ki(****ie*ic**-k*ie*******ic*-k********rk**i;-k*-k***icic*±-k**ic±*****ie  j 

/*  end  of  execution. c  */^ 
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IV.  TACTICAL  LEVEL  SOURCE  CODE 


This  section  contains  source  code  for  the  recovery  path  planner/command 
generator.  The  recovery  path  planner/command  generator  is  responsible  for  planning  a  safe 
path  from  an  arbitrary  AUV  position  into  an  arbitrarily  located  recovery  tube  and 
generating  commands  that  will  be  executed  at  the  execution  level  to  follow  the  planned 
path.  This  file  is  compiled  and  linked  with  other  files  of  the  tactical  level.  All  tatical  level 
software  is  available  online  at 

http:llwww.stl.nps.navy. mill'-brutzmanldissertationlsoftware_reference.html. 

Files  are  available  individually  or  as  a  group  in  a’ .tar  package. 
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/**************i:*ic**ic**ic****^***ic-k-k-k***ic-k*-k**ici,i,ic**ici:ic-k****ic****ic**ic-kiciTicic**** 

* 

*  Filename:  recovery. c 

* 

*  Purpose:  Planner  for  smooth  motion  around  and  into  a  recovery  tube 

*  Reference:  Advanced  Robotics  class  notes,  Dr.  Yutaka  Kanayama 

*  Author:  Duane  Davis 

* 

*  Date:  12  June  96 

* 

*  Language:  ANSI  C 

* 

*  Execution:  tactical 

* 


#include  <stdio.h> 

#include  <stdlib.h> 

#include  <math.h> 

tdefine  TRUE  1 
#define  FALSE  0 
#define  LEFT  -1 
#define  RIGHT  1 

tdefine  DELTA^S  0.01 

tdefine  TUBE_LENGTH  10.0 
tdefine  TUBE_RADIUS  1.75 
tdefine  ARC_DISTANCE  6.0 

/*  COMMANDFILE  also  used  in  tactical. c  */ 
tdefine  COMMANDFILE  "recovery_points . auv" 

tdefine  TUBEFILE  "tube.dat" 
tdefine  DATAFILE  "recovery.dat" 

tdefine  LONGSPACING  6.0 
tdefine  CLOSESPACING  3.0 

/*  Enumeration  for  regions  relative  to  tube  */ 

enum  {  FRONT  =  1,  FRONT_LEFT_CORNER,  LEFT^SIDE,  REAR_LEFT_CORNER,  LEFT  REAR, 
RIGHT_REAR,  REARER IGHT^CORNER,  RIGHT_SIDE,  FRONT_RIGHT_CORNER  }; 

/*  Local  Data  Types  */ 

typedef  struct  Point_type 
{' 

double  X; 
double  y; 

}  Point; 


typedef  struct  Trans formation_type 

Point  point; 
double  theta; 

}  Transformation; 


typedef  struct  Vehicle_state_type 

Transformation  posture; 
double  kappa; 

}  Vehicle_state; 
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/*  File  Scope  Variables  */ 

double  a;  /*  steering  coefficients  */ 
double  b; 
double  c; 

double  station_spacing  =  CLOSESPACING; 
int  delay ed_new_region_output; 
int  first _point_of_plan; 

FILE  *  datafile_ptr;  /*  output  recovery  path  points  to  a  file  */ 

FILE  *  tubefilejtr;  /*  output  tube  points  to  a  file  */ 

FILE  *  commandfile^ptr;  /*  file  containing  AUV  station  commands  */ 

/*  Global  Variables  from  Other  Files  */ 

/*  From  statevector * c  */ 
extern  double  x; 
extern  double  y; 

/*  External  Function  Prototypes  */ 

/*  from  circle. c  */ 

extern  double  normalize_rad  (double  radian_angle) ; 
extern  double  normalize2  (double  degree_angle) ; 

extern  double  degrees  (double  radian_angle) ; 
extern  double  radians  (double  degree_angle) ; 

/***  Local  Function  Prototypes  ***/ 

Transformation  compose  (const  Transformation*  ql,  const  Transformation*  q2); 
Transformation  inverse  (const  Transformation*  q) ; 

Transformation  circular_transform  (double  1,  double  alpha); 
double  direction  (const  Point*  pi,  const  Point*  p2) ; 

int  included_angle  (double  angle,  double  1 owe rebound,  double  upper__bound) ; 

int  normal„to_segment  (const  Point*  p,  const  Point*  seg_pt_l, 

const  Point*  seg_pt_2); 

double  steering_coefficients  (double  sigma,  double*  a,  double*  b,  double*  c) 

double  distance_to_point  (const  Point*  pi,  const  Point*  p2) ; 

double  distance_to_line  (const  Point*  p,  const  Transformation*  line) ; 

double  distance_to_circle  (const  Point*  p,  const  Vehicle_state*  curve); 

double  distance_to_segment  (const  Point  *  p,  const  Point  *  seg_pt_l , 

const  Point  *  seg_pt_2) ; 

double  theta_desired_circle  (const  Point*  p,  const  Vehicle_state*  curve); 

void  step_towards_line  (Vehicle_state*  vehicle,  const  Transformation*  line) ; 

void  step_towards__circle  (Vehicle_state*  vehicle,  const  Vehicle_state*  curve) 

Point  translate_point  (Point*  point,  double  x_trans,  double  y__trans, 

double  rotation) ; 
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void  coitipute__tube_corners  (double  x,  double  y,  double  theta.  Point* 
corner_array) ; 

void  compute_track__lines  (const  Point*  corner_array , 

Vehicle_state*  region2_track. 
Transformation*  region3_track, 
Vehicle_state*  region4_track. 
Transformation*  region5_track, 
Transformation*  region6_track, 
Vehicle_state*  region7_track. 
Transformation*  region8_track, 
Vehicle_state*  region9_track) ; 

int  determine_start_region  (Vehicle_state*  vehicle,  const  Point* 
corner_array )  ; 


int  output_station_point 
target_point)  ; 


(const  Point*  station_pt,  const  Point*  corner_array , 
int  region,  int  *new_region,  const  Point* 


int  recovGry_plan_complete  (const  Vehicle_state*  vehicle, 

double  tube_x,  double  tube_y) ; 


/*  Function  Definitions  */ 
void 

recovery_planner  (tube_x,  tube^,  tube_theta) 
double  tube_x,  tube_j^,  tube__theta; 


double  sigma  =  1.0, 

track_length  =0.0; 
int  vehicle_region, 

new_region  =  TRUE, 
recovery_file__complete  =  FALSE, 
counter; 

Point  corner_array [4 ] , 
tube_center, 

previous_pts[100] ;  /*  memory  of  previous  1'  of  track  */ 

Transformation  regionl,  regions,  regions,  regions,  regions ; 

Vehicle_statG  region2,  region4,  region?,  regions, 
vehicle; 

delayed_new_region_output  =  FALSE; 
first_point_of  jplan  =  TRUE; 

/*  Determine  locations  of  tube  corners  and  paths  to  track  during  recovery  */ 
tube_center .X  =  tube_x; 
tube_center.y  =  tube_y; 

compute_tube_corners  (tube_x,  tube_:7,  radians  (tube_theta) ,  corner_array); 
compute_track_lines  (corner_array,  &region2,  &region3,  &region4,  &region5, 

&region6,  Scregion?,  &region8,  &region9); 
regionl .point .X  =  tube_x; 
regionl .point .y  =  tube_y; 
regionl . theta  =  radians  (tube_theta) ; 

/*  Initial  posture  for  planning  vehicle  at  AUV  location,  pointed  on  track  */ 
vehicle. posture. point .X  =  x; 
vehicle. posture. point .y  =  y; 

vehicle. posture. theta  =  atan2  (tube_/  -  y,  tube_x  -  x) ; 

vehicle. kappa  =  0.0; 

for  (counter  =  0;  counter  <  100;  counter++) 

previous_pts [counter]  =  vehicle .posture .point; 
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steering^coef f icients  (sigma,  &a,  &b,  &c); 

vehicle_region  =  determine_start_region  (&vehicle,  corner_array) ; 
/*  set  up  data  and  command  files  */ 

if  (  ( (datafilejtr  =  fopen  (DATAFILE,  "w"))  ==  (FILE  *)  0)  || 

^  ( (tubefile_ptr  =  fopen  (TUBEFILE,  "w"))  ==  (FILE  *)  0)) 

printf  ("Error  Opening  Recovery  Plan  or  Tube  Data  File\n"); 
return; 


if  { (commandfile_ptr  =  fopen  (COMMANDFILE,  "w"))  ==  (FILE  *)  0) 

printf  ("Error  Opening  Recovery  Plan  Command  FileXn"); 

fclose  (dataf ile_jptr)  ; 

return; 


fprintf  (tubef ile_ptr, "%7 . 41f 
corner_array [0] y) ; 
fprintf  (tubefile_ptr, "%7 ,41f 
corner_array [1]  .y) ; 
fprintf  (tubefile_ptr, "%7 . 41f 
corner_array  [2]  y) ; 
fprintf  (tubef  ile_j)tr,  "%7 . 41f 
corner_array  [3 ]  y )  ; 
fclose  ( tubefile_ptr) ; 


%7.41f\n", 
%7 .41f\n", 
%7.41f\n", 
%7 .41f\n", 


corner_array [0] .x, 
corner_array [1] .x, 
corner_array [2] .x, 
corner_array [3  I .x, 


/*  Output  first  station  point  for  initial  position  */ 

recovery _f  ile__complete  =  output__station_jpoint  (&  (vehicle .posture .point ) , 

corner_array, vehicle_region,  &new_region, 
&:tube_center)  ; 


while  (recovery_file_complete  ==  FALSE) 

{ 

switch  (vehicle_region) 

{ 

case  (FRONT) : 

step_towards_line  (&vehicle,  &regionl); 
if  ( (distance_to_point  (& (vehicle *posture .point) , &tube_center)  < 
(TUBE_LENGTH  /  2.0  +  2.5))  && 

( recovery  _file_complete  ==  FALSE)) 

recovery_file_complete  = 

output_station__point  (& (vehicle. posture  .point)  , 

corner_array,  vehicle_region, 
&new_region,  &tube_center) ; 

track_length  =  0.0; 

}  ■ 

break; 

case  (FRONT_LEFT_CORNER) : 

step_towards_circle  (^vehicle,  &region2); 
if  {normal_to_segment  (& (vehicle. posture. point) , 

^  &corner_array [3] ,  &corner_array ( 0 ] ) ) 

vehicle_region  =  FRONT; 
new_regi on  =  TRUE ; 

} 

break; 

case  (LEFT_SIDE) : 

step_towards_line  (Scvehicle,  &region3); 

if  (  i  normal_to_segment  (& (vehicle. posture. point ) , 

^  &corner_array [0] ,  &corner_array  [1] ) ) 

vehicle_region  =  FRONT_LEFT_CORNER; 
station_spacing  =  CLOSESPACING; 
new_regi on  =  TRUE ; 

} 
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break; 

case  (REAR_LEFT_CORNER) : 

step_towards_circle  {&vehicle,  &region4); 
if  (normal__to_segment  (& {vehicle. posture. point) , 

&corner_array [ 0 ] ,  &corner_array [ 1 ] ) ) 

{ 

vehicle_region  =  LEFT_SIDE; 
station_spacing  =  LONGSPACING; 
new_region  =  TRUE; 

} 

break; 

case  {LEFT_REAR) : 

step_towards__line  (&vehicle,  &region5); 

if  (  !  nonnal_to_seginGnt  (& (vehicle  .posture  .point )  , 

&corner_array [ 1 ] ,  &corner_array [2])) 

{ 

vehicle_region  =  REAR_LEFT_CORNER; 
new_region  =  TRUE; 

} 

break; 

case  (RIGHT^REAR) : 

step_towards_line  (Scvehicle,  &region6)  ; 

if  (  !  normal_to_seginent  (& (vehicle. posture. point ) , 

&corner_array [1] ,  &corner_array [2 ] ) ) 

{ 

vehicl€_region  =  REAR_RIGHT_CORNER; 
new_regi on  =  TRUE ; 

) 

break; 

case  (REAR_RIGHT_CORNER) : 

step__towards_circle  (&vehicle,  &region7)  ; 

if  ( norma l_to_seginent  (& (vehicle. posture. point )  , 

&corner_array [2] ,  &corner_array [ 3 ] ) ) 

{ 

vehicle_region  =  RIGHT_SIDE; 
station_spacing  =  LONGSPACING; 
new_region  =  TRUE; 

} 

break; 

case  (RIGHT_SIDE) : 

step_towards_line  (&vehicle,  &region8); 

if  (  \  normal_to_segment  (& (vehicle .posture .point ) , 

&corner_array [2] ,  &corner_array [3] ) ) 

{ 

vehi c 1 e_regi on  =  FRONT_R I GHT_CORNER ; 
station_spacing  =  CLOSESPACING; 
new_regi on  =  TRUE ; 

} 

break; 

case  (FRONT_RIGHT_CORNER) : 

step_towards_circle  (^vehicle,  &region9); 
if  {normal_to_segment  (& (vehicle. posture. point) , 

&corner_array [ 3 ) ,  &corner_array  [ 0 ] ) ) 

{ 

vehicle_region  =  FRONT; 
new_region  =  TRUE; 

} 

break; 

default:  break; 

} 

fprintf  (datafile_ptr,  "%7.41f  %7.41f\n'', 

vehicle. posture. point .X,  vehicle .posture. point .y) ; 

track_length  +=  DELTA_S; 

if  ( ( (track_length  >=  station_spacing)  II  (new_region) )  kk 
(recovery_f ile_complete  ==  FALSE) ) 

{ 
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/*  Use  previous  point  to  ensure  the  proper  corner  is  the  one  used  */ 
recovery_file_coinplete  =  output_station_point  (& (previous jts  [49]  ) , 

corner_array ,  vehicle^region,  &new_region, 
&tube_center} ; 

track_length  =  0.0; 

} 

for  (counter  =  1;  counter  <  100;  counter++) 

previous_pts [counter]  =  previous_pts [counter-1] ; 
previous_pts [0]  =  vehicle .posture .point ; 

} 

fclose  (datafilejtr)  ; 
fclose  (coinmandf  ile_j)tr)  ; 


/*  Functions  for  working  with  local  data  types  */ 


/*  Compose  two  transformations  and  return  the  result  */ 

Transformation 

compose  (ql,  q2 ) 

const  Transformation*  ql,  *q2 ; 


Transformation  result; 

double  cos_ql_theta  =  cos  (gl->theta) , 

sin_ql_theta  =  sin  (ql->theta); 

result. point .X  =  ql->point.x  +  q2->point.x  *  cos_ql_theta 

-  q2->point.y  *  sin_ql_theta; 
result .point .y  =  ql->point.y  +  q2->point.x  *  sin_ql_theta 

+  q2->point,y  *  cos__ql_theta; 
result. theta  =  ql->theta  +  q2->theta; 

return  (result); 

} 


/*  Compute  the  inverse  of  a  transformation  and  return  the  result  */ 

Transformation 

inverse  (q) 

const  Transformation*  q; 


Transformation  result; 

double  cos_theta  =  cos  (q->theta), 

sin_theta  =  sin  (q->theta); 

result .point .X  =  -q->point.x  *  cos_theta  -  q->point.y  *  sin_theta; 
result. point .y  =  q->point.x  *  sin__theta  -  q->point.y  *  cos_theta; 
result. theta  =  -q->theta; 

return  (result) ; 

} 


/*  Compute  a  circular  transformation  for  length  1  and  deltaTheta  alpha  */ 
Transformation 

circular_transf orm  (1,  alpha) 
double  1,  alpha; 


Transformation  result; 
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double 


alphaSquared  =  alpha  *  alpha, 
alphaFourth  =  alphaSquared  *  alphaSquared, 
alphaSixth  =  alphaSquared  *  alphaSquared  *  alphaSquared; 

result .point .X  =  1  *  (1  -  {alphaSquared  /  6.0) 

+  (alphaFourth  /  120.0) 

-  {alphaSixth  /  5040.0)); 
result .point .y  =  1  *  alpha  * 

{0.5  -  {alphaSquared  /  24.0) 

+  (alphaFourth  /  720.0) 

-  (alphaSixth  /  40320.0)); 

result. theta  =  alpha; 
return  (result); 

} 


/*  Compute  the  direction  from  pi  to  p2  */ 
double 

direction  (pi,  p2) 

const  Point*  pi,  *p2; 


{ 

return  (atan2  (p2->y  -  pl->y,  p2->x  -  pl->x) ) ; 


/*  Determine  whether  an  angle  is  between  two  bounding  angles  */ 
int 

included_angle  (angle,  1 owe rebound,  upper_bound) 
double  angle,  lower^bound,  upper_bound; 


{ 

double  lower_to_upper  =  normalize_rad  {upper_bound  -  lower_bound) , 
lower_to_included  =  normalize_rad  (angle  -  1 owe rebound ) ; 

if  { {lower_to_upper  <  0.0)  && 

{lower_to_included  <=  0.0)  && 

(lower_to_^included  >=  lower_to_upper) ) 
return  (TRUE) ; 

if  ( (lower_to_upper  >  0.0)  && 

{lower_to_included  >=  0.0)  && 

(lower_to_included  <=  lower_to_upper) ) 
return  (TRUE) ; 

if  {lower_to_upper  ==  lower_to_included) 
return  (TRUE) ; 

return  (FALSE) ; 

} 


/*  Determine  whether  a  ray  beginning  at  a  point  normal  to  a  line  through  */ 
/*  two  points  pi  and  p2  will  intersect  the  closed  segment  defined  by  pi  &  p2  */ 
int 

norma l_to_segment  (p,  seg_pt_l,  seg_pt_2) 
const  Point  *p,  *seg_pt_l,  *seg_pt_2; 


{ 

double  dir_p_to_pl  =  direction  (p, seg_pt_l ) ,  /*  Direction  from  p  to  pi  */ 

to_p2  =  direction  (p,  seg_pt_2 )  ,  /*  Direction  from  p  to  p2  */ 

dir_p_t online,  /*  Direction  from  p  to  line  through  pi  &  p2  */ 

dist_p_to_line;  /*  distance  from  p  to  line  through  pi  &  p2  */ 
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Transformation  line; 


line. theta  =  direction  {seg_ptj,  S€gjit_2); 
line,  point  =  *seg_pt__l; 


dist_j)_t online  =  distance_to_line  {p,  &line)  ; 


if  {dist_p__t online  >  0) 

dir_p_to_line  =  normalize^rad  (line. theta  -  M__PI  / 
else  dir_p_to_line  =  normal ize_r ad  (line. theta  +  M_PI 


2.0)  ; 

/  2.0); 


if  (included_anglG  (dir_p_to_line,  dir_p_to_pl,  dir_p_to_p2 )  ) 
return  (TRUE) ; 

return  (FALSE); 


/*  Compute  the  distance  between  two  points  pi  and  p2  */ 
double 

distance__to_point  (pi,  p2) 
const  Point  *pl,  *p2; 


{ 

double  x_dist  =  pl“>x  -  p2->x, 
y_dist  =  pl“>y  “  p2->y; 

return  (sqrt  (x_dist  *  x_dist  +  y_dist  *  y_dist)  )  ; 

} 


/*  Compute  the  signed  distance  from  a  point  to  a  directed  line  */ 
double 

distance_to_line  (p,  line) 

const  Point*  pa¬ 
eons  t  Transformation*  line; 


double  x_min_x0  =  p~>x  ~  line->point .x, 
y_rnin_y0  =  P“>y  -  line“>point .y, 
cos_theta  =  cos  (line'->theta)  , 
sin__theta  =  sin  (line->theta)  ; 

return  (-x_min_x0  *  sin_theta  +  y_min_y0  *  cos_thGta)  ; 


/*  Compute  the  signed  distance ’ from  a  point  to  a  circular  curve  */ 
double 

distance_to__circle  (p,  curve) 

const  Point*  p; 

const  Vehicle_state*  curve; 

{ 

double  x_min_x0 
y_min 
cos_theta0 
sin_thetaO 
k_x_min_x0 
k_y_min_y'0 
numerator_terml 
numerator_term2 
numerator 
denominator_terml 


=  p->x  -  curve”>posture. point. X, 

=  P->Y  -  curve->postur€. point .y, 

=  cos  (curve~>posture. theta) , 

=  sin  (curye~>posture. theta) , 

=  curve->kappa  *  x_min_x0, 

=  curve->kappa  *  y„min_y0, 

=  ~x_min_x0  *  (k_x_min_x0  +  2.0  *  sin_thetaO) 
=  y_min_y0  *  (k_y_min_y0  -  2.0  *  cos_theta0) 
=  numerator_terml  -  numerator_term2, 

=  (k__x_min_x0  +  sin_theta0)  * 

{k__x_min__x0  +  sin__theta0)  ^ 
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denoiTiinator_l:enn2  =  (k_j/_inin_yO  -  cos_thetaO)  * 

(k_y_inin_:yO  -  cos_thetaO)  , 

denominator  =  1  +  sqrt  (denominator_terml  +  denominator_term2 }  ; 

return  (numerator  /  denominator) ; 

) 


/*  Determine  the  unsigned  distance  of  a  point  from  a  line  segment  */ 
double 

distance_to_segment  (p,  seg_pt_l,  seg_pt_2) 

const  Point  *p,  *seg_j)t_l,  *seg_pt_2; 

{ 

Transformation  segment_line; 

double  dist_p__to_pl ,  /*  Distance  from  p  to  pi  */ 

dist_p_to_p2,  /*  Distance  from  p  to  p2  */ 

dist_p_to_line,  /*  Distance  from  p  to  line  through  pi  &  p2  */ 

dir_p_to_pl  =  direction  (p, seg_pt_l) ,  /*  Direction  from  p  to  pi  */ 

dir_p_to_p2  =  direction  (P/ seg_pt_2)  ,  /*  Direction  from  p  to  p2  */ 

dirjp_to_line;  /*  Direction  from  p  to  line  through  pi  &  p2  */' 

segment_line . theta  =  direction  {seg_pt_l,  seg_pt_2); 

segment__line  .point  =  *seg_pt_l; 

dist_p_to_pl  =  distance_to_jpoint  '  (p,  seg_pt_l)  ; 

dist_p_to_p2  =  distance_to_point  (p,  seg_pt_2)  ; 

if  (normal__to_segment  (p,  seg_jpt_l,  seg_pt_2)) 

return  (fabs  {distance_to_line  (p, &segment_line) ) ) ; 

if  (dist_p_to_pl  <  dist_p_to_p2) 
return  (dist_j5_to_pl)  ; 

return  (dist_p_to_p2 ) ; 


} 


/*  Compute  the  tangential  direction  of  a  curve  at  the  image  of  a  point  */ 
double 

theta_desired_circle  (p,  curve) 

const  Point*  p; 

const  Vehicle_state*  curve; 


{ 

double  k_x_min_xO  =  curve- >kappa  *  (P“>x  -  curve->posture. point. x) , 
k_/_min_yO  =  curve- >kappa  *  {p->y  -  curve->posture. point .y) , 
sin_theta  =  sin  (curve->posture. theta) , 
cos_theta  =  cos  (curve->posture. theta) ; 

return  (atan2  (sin_theta  +  k_x__min_xO,  cos_theta  -  k_y_min_yO) )  ; 

} 


/*  Compute  the  steering  coefficients  given  sigma  */ 
double 

steering_coef f icients  (sigma,  a,  b,  c) 
double  sigma,  *a,  *b,  *c; 


{ 

double  k  =  1.0  /  sigma; 
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*a  =  3.0  *  k; 
*b=3.0*k*k; 
*c  =  k  *  k  *  k; 


/*  Move  a  vehicle  one  increment  towards  a  line  */ 
void 

step_towards_line  (vehicle,  line) 

Vehicle_state*  vehicle; 
const  Transformation*  line; 


double  distance  =  distance_to_line  (& (vehicle->posture .point ) ,  line), 
theta_error  =  normalize_rad  {vehicle->posture . theta  -  line->theta) 
dk_ds  =  -{a  *  vehicle->kappa  +  b  *  theta_error  +  c  *  distance) 

delta_theta  =  dk_ds  *  DELTA_S; 

Transformation  incremental_motion; 

/*  Move  straight  towards  the  line  until  we  are  close  */ 
if  (fabs  (distance)  >  4.0) 

{ 

dk_ds  =  0.0; 

delta_theta  =  0.0; 

} 

incremental^motion  =  circular_transform  (DELTA^S,  delta_theta); 
vehicle->posture  =  compose  (&( vehicle- >postu re) ,  Scincremental_motion) ; 


/*  Move  a  vehicle  one  increment  towards  a  circular  curve  */ 
void 

step_towards_circle  (vehicle,  curve) 

Vehicle_state*  vehicle; 
const  Vehicle_state*  curve; 


{ 


double  distance  =  distance„to_circle  (& {vehicle->posture .point ) , 
curve) , 


curve) ) , 


theta_error  =  normal ize_rad  (vehicle->posture . theta 

-  theta_desired_circle  (& (vehicle->posture .point ) , 

kappa_error  =  vehicle->kappa  -  curve->kappa, 

dk_ds  =  - (a  *  vehicle“>kappa  +  b  *  theta_error  +  c  *  distance) 

delta__theta  =  dk_ds  *  DELTA_S; 


Transformation  incrementaljiiotion; 


/*  Move  straight  towards  the  tube  until  we  are  close  */ 
if  (fabs  (distance)  >  4.0) 

{ 

dk_ds  =  0.0; 

delta_theta  =  0.0; 

} 


incremental_motion  =  circular_transf orm  (DELTA_S,  delta__theta) ; 
vehicle- >posture  =  compose  (& (vehicle->posture) ,  &incremental_motion) ; 


/*  Compute  a  2-dimensional  rotation  and  translation  of  a  point  */ 
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Point 

translate_point  (point,  x_trans,  y_trans,  rotation) 
Point*  point; 

double  x_trans,  y_trans,  rotation; 


{ 


double  cos_theta  =  cos 
sin_theta  =  sin 


(rotation) , 
(rotation) ; 


Point  result; 


result. X  =  x_trans 
result. y  =  y_trans 


+  cos_theta 
+  sin__theta 


*  point-* >x 

*  point“>x 


-  sin_theta 
+  cos^theta 


*  point->y; 
point->y; 


return  (result); 

} 


/*  Compute  the  (x,y)  coordinates  of  the  corners  of  the  tube  */ 
void 

compute_tube_corners  (x,  y,  theta,  corner_array ) 

double  X,  y,  theta; 

Point  *  come  r_ar  ray; 


{ 


corner_array  [0] .X  =  -TUBE_LENGTH  /  2.0; 
corner_array  [0]  .y  =  TUBE_RADIUS; 
corner_array [1] .X  =  TUBE„LENGTH  /  2.0; 
corner_array [1 j .y  =  TUBE_RADIUS; 
corner_array  [2 ] . x  =  TUBE_LENGTH  /  2.0; 
corner_array [2] .y  =  -TUBE_RADIUS ; 
corner_array [3 ] .X  =  -TUBE_LENGTH  /  2.0; 
corner_array [3] .y  =  ~TUBE_RADIUS; 


} 


corner_array [ 0 ] 
corner_array [1] 
corner_array [2] 
corner_array [3 ] 


=  translate_point  (&corner_array [0] ,  x,  y,  theta); 
=  translate_point  (&corner_array [1 ] ,  x,  y,  theta); 
=  translate_point  (&corner_array [2 ] ,  x,  y,  theta); 
=  translate_point  (&corner„array [3 ] ,  x,  y,  theta); 


/*  Compute  the  linear  and  circular  paths  along  which  the  vehicle  will  track  */ 
void 

c ompu  t e_t  rack_l i ne s  ( c o rner_a r r ay , 

region2_track,  region3_track,  region4_track,  region5_track, 
region6_track,  region7_track,  region8__track,  region9_track) 


const  Point* 

Vehicle_state* 

Transformation 

VehiclG_state* 

Transformation 

Transformation 

Vehicle_state* 

Transformation' 

Vehicle_state* 


c  o  rne  r_a  r r ay ; 

region2_track; 

region3_track; 

region4_track; 

region5_track; 

reg ion 6_t rack ; 

region7_track; 

regionS^track; 

region9_track; 


/*  Corners  of  Recovery  tube  */ 

/*  Track  around  front  left  corner  */ 
/*  Track  along  left  side  */ 
/*  Track  around  rear  left  corner  */ 
/*  Track  along  left  rear  */ 
/*  Track  along  right  rear  */ 
/*  Track  around  right  rear  corner  */ 
/*  Track  along  right  side  */ 


/*  Track  around  front  right  corner  */ 


{ 

double  arc__curvature  =  1.0  /  ARC_DISTANCE, 

tubG_orientation  =  direction  (&corner_array  [0] ,  5cCorner_array  { 1  ]  )  , 
cos_orientation  =  cos  (tube_orientation) , 
sin_orientation  =  sin  (tube^orientation) ; 


Point  region_2_3_pt,  /* 

region_4_5_pt ,  /* 

region_6_7_pt,  /* 


common  point  on  path  for 
common  point  on  path  for 
common  point  on  path  for 


region  2  and  region  3  */ 
region  4  and  region  5  */ 
region  6  and  region  7  */ 
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region_8_9_pt; 


/*  common  point  on  path  for  region  8  and  region  9  */ 


region_2_3_pt .  x 
regi  on_2__3_p  t .  y 
region_2_3_jpt 


0.0; 

ARC^DISTANCE; 

translate_point  (&region_2__3_pt ,  corner_array[0]  .x, 

corner^array [0] .y,  tube_orientation) ; 


region_4_5_pt.x 
regi  on_4_5_p  t .  y 
region_4__5_pt 


ARC^DI  STANCE; 

0.0; 

translate_point  {&region_4_5_pt ,  corner_array [1 ] .x, 

corner_array [ 1 ] .y,  tube_orientation) ; 


regi  on_6_7_pt .  x 
region_6_7_pt  .y 
region_6_7_pt 


ARC_DISTANCE; 

0.0; 

translate^point  (Scregion_6_7_pt ,  corner_array  [2  ]  .x, 

corner_array [2] .y,  tube_orientation) ; 


region_8_9_pt  .X  =  0,0; 

region_8_9  .y  =  -  ARC_D  I  STANCE; 

region_8_9_pt  =  translate_point  {&region_8_9_pt ,  corner_array  [3  ]  .x, 

corner_array [3 ] .y,  tube_orientation) ; 


/*  Arc  around  the  front  left  corner  of  the  tube  */ 
regi  on2_track~>posture.  point  =  r€gion_^__3_pt; 

regi on2_track“>posture. theta  =  normalize^rad  ( tube_orientation  +  M_PI) ; 
regi on2_t  rack- >kappa  =  arc_curva  ture ; 


/*  Line  parallel  to  the  left  side  of  the  tube  oriented  back  to  front  */ 
region3_track->point  =  region_2_3_pt ; 

region3_track->theta  =  normalize_rad  ( tube_orientation  +  M_PI) ; 

/*  Arc  around  the  back  left  corner  of  the  tube  */ 
regi  on  4_track->pos  ture.  point  =  region_4_5_pt; 

regi on4_track->posture. theta  =  normalize_rad  (tube_orientation  +  M_PI/2.0); 
regi on 4_ t  r ac  k- >kapp a  =  a  r c_cur va  tu  re ; 


/*  Line  parallel  to  the  back  of  the  tube  oriented  center  to  left  */ 
region5_track->point  =  region_4__5_pt  ; 

region5_track“>theta  =  normal! ze_rad  ( tube^orientation  +  M_PI  /  2,0); 

/*  Line  parallel  to  the  back  of  the  tube  oriented  center  to  right  */ 
region6_track->point  =  region_6_7_pt ; 

region6_track->theta  =  normal! ze_rad  { tube_orientation  -  M_PI  /  2.0); 

/*  Arc  around  the  back  right  corner  of  the  tube  */ 
regi on7_t rack- >pos ture. point  =  region_6_7_pt; 

region7_track->posture. theta  =  norraalize_rad  { tube_orientation  -M_PI/2.0); 
region7_track->kappa  =  -arc_curvature; 

/*  Line  parallel  to  the  right  side  of  the  tube  oriented  back  to  front  */ 
region8_track->point  =  region_8_9_pt; 

region8_track->theta  =  normalize_rad  ( tube_orientation  +  M_PI) ; 

/*  Arc  around  the  front  right  corner  of  the  tube  */ 
region9_track->posture. point  =  region_8_9_pt; 

regi on9_track->posture. theta  =  normalize^rad  (tube_orientation  +  M_PI); 
region9_track->kappa  =  -arc_curvature; 


} 


/*  Determine  what  part  of  the  tube  the  AUV  is  closest  to  at  the  start  */ 
/*  i.e.  right  side,  right  back  corner,  back  side,  etc'.  */ 

int 

determiners tart__region  {vehicle,  cornerrarray ) 

VehiclerState*  vehicle; 
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const  Point 


corner_array ; 


/*  Determine  ranges  from  four  corners  and  from  four  sides  */ 
double  dist_corner_array [4 ] , 
dist_segment_array  [4]; 

dist_corner_array [0]  =  distance_to_point  (& (vehicle->posture .point ) , 
&cornGr_array [0 ] ) ; 

dist_corner_array [1]  =  distance_to_point  {& (vehicle->posture .point ) , 
&corner_array [1 ] ] ; 

dist_corner__array  [2 ]  =  distance  to  point  (& (vehicle-->posture  .point ) , 
&corner_array [2 ] ) ; 

dist_corner__array  [ 3  ]  =  distance__to  point  (& (vehicle->posture  .point )  , 
&corner_array [3 ] ) ; 

dist_segment_array [ 0 ]  =  distance_to_segment  (& (vehicle~>posture. point ) / 

&:Corner_array  [0]  , 

&corner__array  [1]); 

dist_segment_array [1 ]  =  distance_to_segment  (& (vehicle->posture. point ) , 

ScCorner_array  [1] , 

&corner_array  [2]); 

dist__segment_array [2 ]  =  distance_to_segment  {& (vehicle->posture. point) , 

&corner_array [2] , 

&corner_array [3])/ 

^ist_segment_array [3 ]  =  distance_to__segmGnt  (& (vehicle->posture .point ) , 

&corner_array [ 3 ] , 

ScCorner_array  [  0  ] )  / 

/*  Determine  what,  segment  or  corner  is  closest  return  appropriate  region  */ 

/*  Also  determine  if  vehicle  is  inside  or  outside  track,  if  it  is  inside  */ 

/*  Turn  it  around  so  that  it  points  away  from  the  tube  */ 

if  (  (dist_segment_array  [3]  <  dist_corner_array  [0 ] )  && 

{dist_segment__array  [3]  <  dist_corner_array  [3  ] )  && 

(dist_segment_array  [3]  <  dist_segment_array  [1]  ) ) 
return  (FRONT) ; 

if  ( {dist_segment__array [3]  ==  dist_segment_array [0] )  && 

(dist_segment_array [3]  ==  dist_corner_array [0] ) ) 

if  {dist_corner_array  [0]  <  ARC__D  I  STANCE) 

vehicle~>posture.  theta  =  normalize^rad  (vehicle->posture.  theta  +]yLPI); 
return  (FRONT_LEFT_CORNER) ; 

} 

if  { (dist_segment_array [0]  <  dist_corner_array [0] )  && 

(dist_segment_array  [0]  <  dist_corner_array  [1  ] )  && 

^  {dist_segment_array  [0]  <  dist_segment_array  [2 ] ) ) 

if  (dist_segment_array [0]  <  ARC_DISTANCE) 

vehicle->posture. theta  =  normalize_rad  (vehicle“>posture. theta  +M_PI); 
station_spacing  =  LONGSPACING; 
return  (LEFT_SIDE) ; 

} 

if  ( (dist_segment_array [0]  ==  dist_segment_array [1 ] )  && 

{dist__segment_array  [0]  ==  dist_corner_array  [  1  ]  ) ) 

if  (dist_corner_array [1]  <  ARC_DISTANCE) 

vehicle->posture. theta  =  normalize^rad  (vehicle->posture. theta  +M_PI); 
return  (REAR_LEFT_CORNER) ; 

} 

if  ( (dist_segment_array [1]  <  dist_corner_array [1 ] )  && 

(dist_segment_array [1]  <  dist_corner_array [2 ] )  && 

(dist_corner_array  [1]  <  dist_corner_array  [2  ] ) ) 

if  (dist_segment_array [1]  <  ARC_DISTANCE) 
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vehicle->posture. theta  =  normalize_rad  {vehicle->posture. theta  +M_PI); 
return  (LEFT^REAR) ; 

} 

if  { (dist_segment_array [1]  <  dist_corner_array [1 ] )  && 

{disc_segment_array [1]  <  dist_corner_array [2 ] )  && 

(dist_corner_array [2 ]  <=  dist_corner_array [1 ] ) ) 

{ 

if  (dist_segment_array [1]  <  ARC_DISTANCE) 

vehicle->posture. theta  =  normalize^rad  (vehicle->posture , theta  +M  PI); 
return  (RIGHT^REAR) ; 

} 

if  ( (dist„seginent_array  [2]  ==  dist_segment_array  [1  ] )  && 

{dist_seginent_array  [2]  ==  dist_corner_array  [2]  )  ) 

{ 

if  {dist__corner_array  [2]  <  ARC_DISTANCE) 

vehicle-* >posture. theta  =  normal ize_rad  (vehicle->posture. theta  +M_PI); 
return  {REAR_RIGHT_CORNER) ; 

} 

if  { (dist_segment_array [2]  <  dist_corner_array [2 ] )  && 

{dist_segTnent_array  [2]  <  dist_corner_array  [3  ] ) ) 

{ 

if  {dist_segment_array[2]  <  ARCJISTANCE) 

vehicle->posture. theta  =  nonnalize_rad  (vehicle->posture. theta  +M_PI); 
station_spacing  =  LONGSPACING; 
return  (RIGHT_SIDE) ; 

} 

if  (  (dist_seginent_array  [2]  ==  dist_seginent_array  [3  ] )  && 

^  (dist_segment_array  [2]  ==  dist_corner_array  [3]  ) ) 

if  {dist_corner_array  [3]  <  ARC_DISTANCE) 

vehicle->posture. theta  =  normalize_rad  (vehicle->posture. theta  +M_PI); 
return  {FRONT_RIGHT_CORNER) ; 

} 

return  (0) ; 

} 


/*  Output  an  appropriate  command  for  the  AUV  station  point  relative  to  tube  */ 
int 

output_station_point  {station_pt,corner_array,  region^  new_region,  final_point) 
const  Point  ^station _jpt,  *corner_array; 
int  region,  *new_region; 
const  Point  *f inal_point; 


static  int  region_l_station_pt  =  1; 

double  tube_heading  =  degrees  (direction  (&corner_array [0] , 

&corner_array [1] ) ) ; 

char  command_string  [80]  =  "EDGE-STATION  ", 
buffer  [25]; 

double  range,  /*  range  from  vehicle  to  station  keeping  corner  */ 

bearing;  /*  bearing  from  vehicle  to  station  keeping  corner  */ 

int  sonar__scan_di recti on;  /*  scan  direction  to  acquire  correct  corner  */ 
int  return_value  =  0; 

switch  (region) 

{ 

case  (FRONT) : 
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/*  Final  Recovery  --  Sonars  to  side,  move  straight  into  tube  */ 
if  (distance_to_point  (station__pt,  f inal__point )  <= 

{TUBE_LENGTH  /  2.0  +  2.5)) 
return_value  =  1; 

/*  Coming  from  left  side,  use  right  corner  for  positioning  */ 
if  (region_l_station_pt  ==  4) 

{ 

sonar_scan_direction  =  LEFT; 

range  =  distance_tojpoint  (station^pt,  &corner_array [3 ] ) ; 
bearing  =  degrees  (direction  (station_pt,  &corner_array [3 ] ) ) ; 


/*  Coming  from  right  side,  use  left  corner  for  positioning  */ 
else 


{ 

sonar_scan_direction  =  RIGHT; 

range  =  distance_to joint  (stationjt,  5ccorner_array[0]); 
bearing  =  degrees  (direction  (stationjt,  &corner_array  [0 ] ) )  ; 

break; 

case  (FRONT^LEFTJORNER)  : 

sonar_scan_direction  =  LEFT; 
if  ( *new_region) 

{ 


range  =  distance_to_point  (stationjt,  &corner_array  (0  ] )  ; 
bearing  =  degrees  (direction  (stationjt,  &corner_array  [0  ] )  }  ; 

else 

{ 

range  =  distance_to joint  (stationjt,  ^corner jrray  [3 ] )  ; 
bearing  =  degrees  (direction  (stationjt,  &corner  jrray  [3  ] )  )  ; 


regionJ_stationjt  =  4; 
break; 

case  (LEFT_SIDE) : 

sonar_scan_direction  =  LEFT; 

range  =  distancejo joint  (stationjt, 

bearing  =  degrees  (direction  (stationjt, 

break; 

case  (REARJEFT_CORNER)  : 

sonar jcan_direction  =  RIGHT; 

range  =  distance Jojoint  (stationjt, 

bearing  =  degrees  (direction  (stationjt, 

break; 

case  (LEFT_REAR) : 

sonar jcan_direction  =  RIGHT; 

range  =  distancejojoint  (stationjt, 

bearing  =  degrees  (direction  (stationjt, 

break; 

case  (RIGHTJEAR)  : 

sonarjcan_di recti  on  =  LEFT; 

range  =  distancejojoint  (stationjt, 

bearing  =  degrees  (direction  (stationjt, 

break; 

case  (REARJIGHTJORNER)  : 

sonar_scan_di recti on  =  LEFT; 

range  =  distancejojoint  (stationjt, 

bearing  =  degrees  (direction  (stationjt, 

break; 

case  (RIGHTJIDE)  : 

sonarjcan_direction  =  RIGHT; 

range  =  distancejojoint  (stationjt, 

bearing  =  degrees  (direction  (stationjt, 

break; 

case  ( FRONT JIGHTJORNER)  : 

sonarjcan_direction  =  RIGHT; 
if  (*newjegion) 


&corner_array [0] ) ; 
Sccornerjrray  [0]  ) )  ; 


&corner__array  [2] ) ; 
&corner_array [2] ) ) ; 


&corner  jrray  [  2  ] ) ; 
&corner_array [2] ) ) ; 


&corner  jrray  [  1  j  )  ; 
&cornerjrray  [  1  ]  )  )  ; 


^corner  jrray  1 1 1  )  ; 
&corner  jrray  [  1  ]  )  )  ; 


^corner  jrray  [  3  ] ) ; 
acorner  jrray  [  3  ] )  ) ; 
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{ 

range  =  distance_to_point  (station_pt,  &:Corner_array  [3  ] )  ; 
bearing  =  degrees  (direction  (station_pt,  &corner_array [3 ] ) ) ; 

} 

else 

{ 

range  =  distance_to_point  (station_pt,  &corner_array [0 1 ) ; 
bearing  =  degrees  (direction  (station_pt,  &corner_array [0 1 ) ) ; 

} 

region_l_station_pt  =  In¬ 
break; 
default : 

break; 


/*  First  station  in  region,  make  sure  AUV  aquires  correct  corner  */ 
if  ( ( (*new_rGgion)  && 

(region  !=  FRONT_RIGHT_CORNER)  && 

(region  !=  FRONT_LEFT_CORNER) )  II 
(dGlayed_new_region__output)  I  | 

( first jpoint_of_plan) ) 

{ 

sprintf  (buffer, "%lf  %lf  ", 

range,  (bearing  +  sonar_scan_di recti on  *  5.0)); 
strcat  (command__string,  buffer)  ; 

*nGW_region  =  FALSE; 
delayed_new_region__output  =  FALSE; 
first_point_of_plan  =  FALSE; 

} 

else  if  (*new_region) 

{ 

delayed_new_region_output  =  TRUE; 

*new_region  =  FALSE; 


/*  Print  desired  range  and  bearing  to  string  */ 
sprintf  (buffer, "%lf  %lf",  range,  bearing); 
strcat  (command_string,  buffer); 

/*  If  in  region  1,  ensure  AUV  points  in  direction  of  tube  */ 
if  ( (region  ==  FRONT)  { I 

((region  ==  FRONT_RIGHT_CORNER)  && 

(fabs  (normalize2  (bearing  -  tube__heading) )  <  45.0))  M 
((region  ==  FRONT_LEFT_CORNER)  && 

(fabs  (normalize2  (bearing  -  tube_heading) )  <  45.0))) 

{ 

sprintf  (buffer,"  %lf",  tube__heading)  ; 
strcat  (coiTimand__string,  buffer)  ; 


/*  Print  AUV  command  to  file  */ 

fprintf  (commandfile_ptr,"%s\n",command_string) ; 

if  (return_value  ==  1) 

{ 

sprintf  {command_string, "ENTER-TUBE  %lf  %lf", 

distance_to_point  (station_pt,  final_point) , 

degrees  (direction  (&corner_array [0] ,  &corner__array [1] ) ) ) ; 

fprintf  (commandfile_ptr,"%s\n",command_string) ; 

} 

return  { return_value) ; 
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/*  Determine  when  the  recovery  path  plan  has  been  completed  */ 
int 

recovery_plan_complete  (vehicle,  tube_x,  tube_y) 


const  Vehicle_state*  vehicle; 
double  tube_x,  tubej'; 


double  x_dist  =  vehicle->posture .point .x  -  tube_x, 
y_dist  =  vehicle~>posture.point .y  -  tube_y, 
distance  =  sqrt  (x_dist  *  x_dist  +  y_dist  *  y 

if  (distance  <  DELTA_S) 
return  (TRUE) ; 

return  (FALSE); 


/*  End  of  File  recovery. c  */ 


_dist)  ; 
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V.  MISSION  GENERATION  EXPERT  SYSTEM  SOURCE  CODE 


This  section  contains  source  code  for  the  mission  planning  expert  system.  Source 
consists  of  three  files:  mission_expert.pl,  mission_cpp.c  and  mission_pl.c.  The  file 
mission_expert.pl  is  written  in  Prolog  and  requires  Quintus  Prolog  version  3.2  and  XPCE 
for  X- windows  version  4.1  (Prowindows).  The  files  mission_cpp.c  and  mission_pl.c  are 
written  in  C  and  can  be  compiled  on  any  platform.  These  fiiles  do  however  require  script 
files  (controller_pl.script,  controller_standalone_pl.script,  and  controller_cpp. script)  that 
are  available  online.  These  source  and  script  files  are  available  online  individually  or  as 
part  of  the  Phoenix  AUV  software  package  in  .tar  format  at 
http:llwww.stl.nps.navy.mill~bmtzmanldissertationlsoftware_reference.html 
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%  File  : 
%  Author  : 
% 

%  Project: 
% 

%  Purpose: 


System 


Use 


Date 


mission_expert.pl 

Duane  Davis,  Brad  Leonhardt 

Pheonix  AUV  Strategic  Level  Mission  Generation  Expert  System 

Mission  planning  and  generation  expert  system  for  the  Phoenix 
autonomous  underwater  vehicle 

Quintus  Prolog  3.2  with  pwxpce  3.0  { Prowindows) 

ai4/usr/work:3/auv/strategic>  newprowin 
[mission_expert ] . 

?-  go. 

16  May  96 


%  NOTE:  This  system  only  works  on  ai4.  To  run  from  another 
%  machine,  you  must  telnet  to  ai4  and  open  a  remote  xterm. 


ensure_loaded (library (math) ) , 
ensure_loaded(vehicle_info) . 
unknown(A, fail) . 

dynamic  phase/5,  start^hase/1 ,  phase_list/l,  current_phase/l, 
complete_successor/l,  entry_mode/l,  pathobject/1 ,  abort_successor/l, 
x_scale/l,  x_2ero/l,  y_scale/l,  y_2ero/l,  searchpoint/1 ,  goal_order/l, 
start_state/l,  me_steps/l,  next_phase/l,  last_point/2 , 
phas e_summary__l  i  s t  / 1 ,  tube_da t  a  /  4 . 

go  :-  quit,  abolish {phase/5 ) ,  abolish (start_phase/l) ,  abolish (phase_list/l ) , 
abolish (entry _mode/l) ,  asserta (phase_list ([])),  initial_menu . 


%  File  : 
% 

%  Author  : 
% 

%  Project: 
% 

%  Purpose: 
% 

% 

% 

%  System  : 
% 

%  Use  : 
% 

% 


control .pi 

Duane. Davis,  Brad  Leonhardt 

Pheonix  AUV  Strategic  Level  Mission  Generation  Expert  System 

This  file  contains  routines  for  drawing  the  initial  system 
menu,  controlling  the  overall  flow  of  the  program,  and 
executing  the  phase  by  phase  mission  generation  facility. 

Quintus  Prolog  3.2  with  pwxpce  3.0  (Prowindows) 

ai4/usr/work3/auv/strategic>  newprowin 
?-  [mission_expert] , 

?-  go. 


%  Date  :  26  April  96 


initial_menu  make_main_menu,  make _phase_menu_labels . 

/*  make_main_menu  creates  a  dialog  box  with  choices  for  creating,  modifying, 
/*  and  deleting  phases,  and  for  getting  means  end  help  to  plan  a  mission. 

/*  A  chart  of  the  operating  area  is  also  displayed  with  point  and  click 
/*  capability  for  entering  navigation  points  */ 

make_main_menu  :-  destroy_start_window_objects,  fail. 
make_main_menu 

new (@start_dialog, dialog {'Options' ) ) , 
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new ( Start^menu, menu { 'Available  Operations  ',choice)), 
send (Star t_jnenu, layout , vertical) , 
new(Quit,button( 'Quit' , message (@prolog, quit) ) ) , 

new (Make , button {' Generate  Mission  Code' , message (@prolog, spec_complete) )) , 
new((ifile_name,text_item( 'Output  File  Name  ',")), 
new(Maps,menu( 'Available  Charts  cycle)), 
send (Maps , layout , vertical ) , 

construct_menu (Maps, ['Moss  Landing' , 'Test  Tank', 'NFS  Pool' ] , get_map) , 
new (Label, label) ,  send (Label , selection, 

'Coordinates  from  upper  left  corner.'), 
new(P,  path),  assertz (pathobject (P) ) ,  send (P,pen, 2 ) , 
new(@path_xl,text_i tern ('Chart  X  ',0)), 
new(@path_yl,  t  ex  t__i  tern  ('Chart  y  ',0)), 

new (Path_clear , button ( 'Clear  Path' , message (@prolog, erase_paths ) ) ) , 
new (©picture, picture) ,  send (©picture, size, size (400 , 800 ) ) , 
new (©bmp, bitmap) , 

send (©picture, clear) ,  send {©picture, display , ©bmp) , 
erase_paths,  send (©picture, display , @mouse_click_path) , 
send (©star t_dialog, size, size (800 , 150} ) , 
construct_menu (Start_menu, [' Phase  by  Phase  Generation', 

'Modify  Current  Mission', 

'Means  Ends  Generation ', 'Create  Initialization  File' ] , initial_choice) , 
send  (©started!  a  log,  append;  Start__menu) , 

send(©file_name, right, Start_raenu) ,  send (Maps , right , @file_name) , 
send  (Qui  t ,  below,  Start__menu ) ,  send  (Make ,  right ,  Qui  t )  , 
send {Path_cl ear, right, Make) , 

send (©path_xl, right, Pa th_clear) ,  send (@path_yl , right, ©path_xl) , 
send (©picture, below, ©start_dialog) ,  send (©start_dia log, open) , 
get_map ( 'Moss  Landing' ) , 
send (©picture,  rfecogniser, 

handler  {ms_left_down,  mess  age  (©pro  log,  left_button_down,©argl) ) ) , 
send {©picture,  recogniser, 

handler  (ms_right_down,message  (©prolog,right_button__down,@argl) )  )  . 


/*  make_phase_menu  creates  a  dialog  box  which  is  used  for  displaying 
/*  the  names  of  phases  as  they  are  specified.  Each  phase  is  displayed 
/*  with  its  complete  and  abort  successor  phase  names  */ 

make_phase_menu_labels  : - 

pad_to_30(  'SPECIFIED  PHASES' , Phasejabel ) , 

pad_to_3  0 { ' COMPLETE  SUCCESSORS ' , Comp_label ) , 

pad_to_30  ( 'ABORT  SUCCESSORS'  ,Abort_label )  , 

new (©phases, label) ,  send {©phases, selection, Phase_label) , 

new (@c_succ,. label)  ,  send{@c_succ,  se lection, Compel abel)  , 

new (©a_succ, label) ,  send (©a_succ, selection, Abo rt_label ) . 


/*  Display  Phase  Menu  Opens  the  Phase  Summary  Window  and  */ 

/*  Fills  it  puts  labels  for  all  of  the  phases  into  it  */ 

display_phase_menu ( [ [header , Phases, Completes, Aborts ]  I  Rest] ) 
free (@phase_dialog) , 

new (@phase_dialog, dialog { ' Phase  Summary' ) ) , 
send (@phase_dialog, append, Phases) , 
send (Completes, right, Phases) , 
send (Aborts, right, Completes) , 
abolish (phase_summary_list /I) , 

asserta(phase_summary„list ( [ [header, Phases, Completes, Aborts]  1  Rest] ) ) , 
fail. 

display_phase_menu(  [  [Namel ,  Phasel ,  Complet el ,  Abort  1] , 
[Name2,Phase2,Complete2,Abort2]  I  Rest]) 

new (Phase_label, label) ,  send (Phase_label, selection, Phase2) , 

new {Complete_label, label) ,  send (Complete_label , selection, Complete2 ) , 

new (Abort_label, label) ,  send (Abort_label , selection, Abort2) , 

send {Phase_label, below, Phasel) , 

send {Complete_label, right, Phase_label) , 

send (Abort_label , right , Complete_label ) , 
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display_phase_nienu ( [ [Narae2 , Phase_label , Complete_label, Abort_label] 
i  Rest] ) . 

display_phase_menu ( [_] )  send (@phase_dialog, open) . 

/*  Alternates  between  moss  landing,  test  tank,  and  NFS  pool  */ 

/*  maps  depending  on  menu  selection  from  main  dialog  box  */ 

get_map(_)  erase_paths,  fail. 
get_map ( "Moss  Landing")  : -  send {@bmp, load, xymoss) , 
send (©picture, display, @bmp) ,  consult (moss_info) . 
getjnap { 'Test  Tank')  send (©bmp, load, tank) , 

send (©picture, display, ©bmp) ,  consult (tank_info) . 
get_map('NPS  Pool')  send {©bmp, load, nps_pool ) , 
send (©picture, display, ©bmp) ,  consult (pool^info) . 

/*  Menus  for  Creation,  Deletion,  and  Modification  of  Phases  * / 

/*  Phase  by  Phase  Mission  Creation  Menu  */ 

/*  phase_generation_jnenu  makes  a  dialog  box  to  get  the  type  of  phase 
/*  that  the  user  wishes  to  enter.  Types  of  phases  are: 

/*  Change  Depth,  Transit,  Hover,  GPS  Fix,  Rotate  AUV  Search, 

/*  Rotate  Sonar  Search,  Change  Course,  and  Wait  */ 

phase_generation_menu  : -  reset_to_main_menu, 
de s t r oy_ph a s e_en t ry _ob jects,  fail. 
phase__generation_menu  :  -> 

new (©type_dialog, dialog ( 'Phase  Type' ) ) , 

new(Menu,menu( 'Press  button  for  next  operation' , choice) ) , 

send (Menu, layout , vertical) , 

new (Cancel , button ( 'Cancel" ,message (©prolog, reset_to_main_menu) ) ) , 
cons true t_menu (Menu, ['Depth  Change', 'Course  Change', 'Transit', 'Hover', 
"GPS  Fix', 'Rotate  Sonar  Search' , 'Rotate  AUV  Search' , 'Wait ' , 
'Recover  in  Tube' , "Modify  Phase' , 'Delete  Phase' ], phase_info) , 
send (©type_di a log, append, Menu) , 
send (Cancel , below, Menu) ,  send (©type_dialog, open) . 


/*  Get  input  specific  to  different  phase  types  */ 

/*  phase_info  is  used  when  creating  or  modifying  phases  to  get 
/*  information  specific  to  each  type  of  phase  (as  well  as 
/*  information  general  to  all  types)  */ 

phase_info( 'Delete  Phase')  \+phase , 

invalid_option_report (no_phases) . 
phase_info  ( 'Delete  Phase")  delete_phase . 

phase_info{  "Modify  Phase")  \+phase  , 

invalid_option_report (no^phases) . 
phase_info ( 'Modify  Phase')  modify_phase . 

phase_info(_)  destroy_phase_entry_ob jects,  make_common_i terns,  fail. 

phase_inf o ( 'Depth  Change' )  : -  asserta (current_phase ( 'Depth  Change' ) ) , 
new ( ©parameter_dialog, dialog { ' Depth  Change  Parameters ' ) ) , 
max_vehlcle_depth  (MaxZ) , 

new(©new_depth,slider  ( 'New  Depth  ' ,  0,MaxZ,  0 ) )  , 
send  (©parameter_dialog,  append,  ©phase_name)  , 
send (©new_depth, below, @phase_name) , 

send (©time_out, below, @new_depth) ,  di splay_common_i terns . 
phase_info( 'Course  Change')  asserta (current_phase ( 'Course  Change')), 
new (©parameter^dialog, dialog ('Course  Change  Parameters')), 
new{©heading,slider ( 'New  Course  ',0,360,0)), 
send (@parameter_dialog, append, ©phase_name) , 
send {©heading, below, @phase_name) , 

send {©time_out, below, ©heading) ,  display_common_i terns . 
phase_info( 'Wait' )  asserta (current_j)hase ( 'Wait ' ) ) , 
new (@parameter_dialog, dialog ( 'Wait  Parameters')) , 
send (@parameter_dialog, append, @phase_name) , 
send (@time_out, below, @phase_name) ,  display_common_items . 
phase__info{  "Transit' )  asserta (current_phase ( 'Transit ' )  )  , 
new(©parameter_dialog,dialog( 'Transit  Parameters' ) ) , 
send (©parameter_dialog, append, @phase_name) , 
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makG_x_^_depth_i  terns , 
di  splay_x_y_depth_i  terns , 

send(@tiine_out, below, @new_jy) ,  display^common^items . 
phase_info( 'Hover')  asserta {current_phase ( 'Hover ' ) ) , 

new (©parameter^dialog, dialog ('Hover  Parameters') ), 
send (@parameter_dialog, append, @phase_namG) , 
new(.@heading,slider(  'Heading  ',0, 360,0)), 
ma  ke_x_y_dep  t h_i terns , 
di  splay_x_y_depth_i  terns , 
send (©heading, below, ©new^y) , 

send {©tiine_out, below, ©heading) ,  di splay_common_i terns . 
phase_info( 'GPS  Fix')  asserta (current_phase ( 'GPS  Fix')), 
new (©parameter_dialog, dialog { 'GPS  Fix  Parameters')), 
send {©parameter_dialog, append, ©phase_name) , 
send (@time_out , below, ©phase_name) ,  di spl ay _common_i terns . 
phase_info { 'Rotate  Sonar  Search') 

asserta (current_phase{ 'Rotate  Sonar  Search')), 
new(@parameter_dialog,dialog('Rotate  Sonar  Search  Parameters')), 
send (©parameter^dialog, append, ©phase_name) , 
make_x_j/_depth_i terns ,  display_x_^_depth_i terns , 
send  (©time_out ,  below,  ©new__y)  ,  display__common_i  terns  . 
phase_info ( 'Rotate  AUV  Search') 

asserta {current_phase ( 'Rotate  AUV  Search')), 
new(@parameter_dialog,dialog('Rotate  AUV  Search  Parameters')), 
send (©parameter_dialog,  append,  @phase__name) , 
makG_x_/_dep th_i  terns ,  di splay_x_^_dep th_i  terns , 
send  (©time_out,  below,  ©new_y)  ,  display_common_i  terns  . 
phase_info ( 'Recover  in  Tube')  asserta (current_phase { 'Recover  in  Tube')), 
new (@parameter_dialog, dialog { 'Tube  Recovery  Parameters' ) ) , 
send{©parameter_dialog, append, ©phase_name) , 
make_x_/_depth_i  terns , 
send(@new_x, name, 'Tube  X  Position  '), 
send (©new_y, name, 'Tube  Y  Position  '), 
send (©new_depth, name, 'Tube  Depth  ' ) , 
di  splay_x^_depth_i  terns , 

new (©heading, slider ('Tube  Entry  Heading  ',0,360,0)), 
send (©heading, below, ©new_y) , 

send (©time_out, below, ©heading) ,  di splay_common_i terns . 
phase_inf  o  {_)  :  -  display_common_i terns . 


/*  make_common_i terns,  display_common__i terns,  make_x_y_depth_i terns,  and 
/*  display_x_^_depth__i terns  are  used  for  creating  and  displaying  buttons 
/*  and  text  items  for  information  and  operations  common  to  multiple 
/*  types  of  phases  */ 

make_common_i terns  new (©phase_name, text__item{ 'Phase  Name  ','')), 
new(@time_out, slider ( 'Time  Out  ',0,500,500)), 
new(@done, button ( 'Done' , message (©prolog, assert_phase) ) ) , 
new (©reset, button ( 'Reset  Phase', 
message(@prolog,destroy_phase_entry_objects) )) . 

display_common_i terns  phase_abort_successor  (Abort_successor) , 

phase__complete_successor (Complete_successor) , 
send (Complete_successor, below, ©time_out ) , 
send (Abort_succ€ssor , right , Complete_successor) , 
send (©done, below, Complete_successor) ,  send (©reset, right, ©done) , 
send (©parameter_dialog, open) . 

make^x^/^dep t h_i terns 

max_vehicle_dGpth  (MaxZ) , 

new (©new_depth, slider ( 'Depth  '  ,0,MaxZ,0) )  , 

get (©path_xl , selection, X) ,  get (©path^^l, selection, Y) , 

op_area (Xmin, Ymin,Xmax, Ymax) , 

new (@new_x, slider ('X  Position  ' ,Xmin, Xmax,X) ) , 

new(©new_7, slider ( 'Y  Position  ' , Ymin, Ymax, Y) ) . 
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display_x_jy_depth_iteins  send {@new_depth,  below,  @p ha se_name)  , 

send(@new_x, below, @new_dGpth) , 
send (@new_y, below, @new_x) . 


/*  phase_abort_successor  and  phase_complete_successor  create  menus  to  enter 
/*  the  phases  that  will  follow  the  phase  being  specified  or  modified  */ 
phase_abort_successor (Abort_successor)  : - 

new (Abort_successor, menu ("Phase  Abort  Successor  ",choice)), 
send (Abort_successor , layout , vertical ) , 
phase__list  (List)  , append (  [  "Unspecified"  I  List] , 

[  "mission__complete' ,  'mission_abort '  ]  ,List2  )  , 
construct_menu (Abort_successor,List2 ,handle_abort_successor) . 

phase_complete_successor (Complete_successor) 

new (Coinplete__successor, menu ( "Phase  Complete  Successor  ",choice)), 
send(Complete_successor, layout , vertical) , 
phase_list (List) , append ([ "Unspecified"  I  List], 

[  "inission_complete" ,  "mission_abort '  ]  ,List2 ) , 
construct_menu (Complete_successor, List2 , handle_completG_successor ) . 

handle_abort_successor(_)  .retract (abort_successor (X) ) ,  fail. 

handle_abort_successor( "Unspecified" )  get_unspecified_phase (abort) . 
handle_abort_successor (Successor)  asserta (abort_successor (Successor) ) . 

handle_complete_successor (_)  retract (complete^successor (X) ) ,  fail. 
handle_complete_successor{ "Unspecified")  get_unspecified_phase (complete) . 
handle_complete_successor (Successor) 

asserta (complete_successor (Successor) ) . 


/*  Modify  a  previously  specified  phase  */ 

/*  modify_phase  provides  a  menu  with  the  names  of  all  specified  phases 
/*  for  the  user  to  choose  from 
modify_phase  destroy_phase_entry_objects, 

abolish (entry __mode/l ) ,  asserta (entry _mode (modify )) ,  fail. 
modify_phase  new (©modify __dialog, dialog (" Phase  Modification ')) , 

new (Menu, menu ( "Press  button  for  phase  to  modify ",  choice) ) , 
send (Menu, layout , vertical) , 

phase_list (Phases) ,  cons true t_menu (Menu, Phases, modi fy_phase) , 

new (Reset, button ( "Reset" , message (@prolog,destroy_phase_entry_objects) ) ) , 

send  ( ©modi fy_dialog,  append, Menu) , 

send (Reset, below, Menu) ,  send (©modi fy_dia log, open) . 

modi fy_phas e ( Phase)  : -  phase (Phase, Type, Parameters, CSuccessor,ASuccessor) , 
phase_info (Type) ,  asserta (complete_successor (CSuccessor) ) , 
asserta(abort_successor(ASuccessor) ) ,  replace_parameters (Phase, Type, Parame¬ 
ters)  . 


/*  Replace  parameters  marshals  all  of  the  previously  specified  parameters 
/*  for  a  phase  and  places  it  in  the  appropriate  places  in  the  data  entry 
/*  dialog  when  a  phase  is  being  modified  so  that  the  data  does  not  have  to 
/*  be  entered  from  scratch  */ 

replace_parameters{Name,_,_)  send (©phase_name, selection, Name) ,  fail. 
replace_parameters (_, "Course  Change' , [Course,  Time_out] ) 

send (©heading, selection, Course) ,  send (©time_out, selection, Time_out) . 
replace_parameters (_, "Depth  Change' , [Depth,  Time_out] ) 

send (@new_depth, selection, Depth) ,  send (@time_out, selection, Time_out) . 
replace_parameters(_, "Transit", [X,  Y,  Depth,  Time_out]) 
send (@new_x, selection, X) ,  send (©new_y, selection, Y) , 
send (©new_depth, selection, Depth) ,  send (@time_out, selection, Time_out) . 
replace_j)arameters (_, 'Hover' ,  [X,  Y,  Depth,  Heading,  Time-out]) 
send  (©new_x,  selection,  X)  ,  send(©new^,  selection,  Y)  , 
send (@new_depth, selection, Depth) , 

send {©heading, selection, Heading) ,  send (©time_out, selection, Time_out) . 
replace^parameters {_, 'GPS  Fix" , [Time_out] ) 
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s end ( @ t ime^ou t, select! on, Time^ou t )  . 
replace_parameters(_, 'Rotate  Sonar  Search',  [X,  Y,  Depth,  Tiine__out]) 
send {@new_x,  selection, X) ,  send  ((inew_y,  selection,  Y) , 
send (@new__depth,  selection.  Depth) ,  send (@time_out,  selection, Time__out)  . 
replace_paraineters(_, 'Rotate  AUV  Search' ,  [X,  Y,  Depth,  Tiine_out]) 
send  (@new_x,  selection, X)  ,  send  ((anew__y,  selection,  Y)  , 
send (@new_depth, selection, Depth) ,  send{@tiine_out, selection, Time_out) . 
replacejarameters(_,  'Wait' ,  [Tinie_out] ) 
send  (@tiine_out ,  selection,  Tiine_out)  . 

/*  Delete  a  previously  specified  phase  */ 

/*  delete_phase  provides  a  list  of  all  specified  phases  for  the  user 
/*  to  choose  from  */ 

deletG_phase  :  -  dest:roy_phase_entry_objects, 

abolish (entry_mode/l) ,  asserta (entry _mode (delete) ) ,  fail . 
delete_phase  new (@delete_dialog, dialog (' Phase  Deletion')), 
new (Menu, menu ( 'Press  button  for  phase  to  delete  ',choice)), 
send (Menu, layout , vertical ) , 

phase_list (Phases) ,  construct__menu (Menu, Phases, delete_phase) , 
new (Reset, button ('Reset' ,message (@prolog, destroy _phase_entry_ob jects ) ) ) , 
send (@delete_dialog, append, Menu) , 
send(ResGt, below, Menu) ,  send (©dele te__dialog, open) . 
deletG_phase( Phase)  retract (phase (Phase, ) , 
pad_to_30 (Phase, Phase_label) , 
phase_summary_list (OldPSList ) , 

delete { [_,Phase_label,_,_] , OldPSList ,NewPSList) , 
display_phase__menu  (NewPSList ) , 

retract (phase_list(PList) ) ,  delete (Phase, PList,NewPList) , 
asserta  (phase_list  (NewPList)  ) ,  destroy_ptaase_entry_objects . 


%  Append  new  point  to  most  recent  path  when  left  button  goes  up; 
%  first  path  object  will  be  the  most  recent 
left_button_down (Event)  : - 

Gxtend_mouse_click_path (Event , X, Y) , 
update_position_menus (X,Y) . 

r ight_bu t  ton_down ( Event )  : - 

ext end__mou  se_c  lie k_p  a  t  h  ( Even  t ,  X ,  Y )  , 
update_transit_point__menus  (X,Y)  . 

extend_mouse_click_path (Event, Xrounded,Yrounded) 
get (Event, x,XPointer) ,  get (Event ,y, YPointer) , 
x_zero (X_2ero) , x_scale (X_scale) , 

Xcorrect  is  ( ( (XPointer  -  X_zero)  /  (X^scale) )  +  0.5), 
y__zero  (Y_zero)  ,  y__scale  ( Y_scale)  , 

Ycorrect  is  (((YPointer  -  Y_zero)  /  (Y__scale)  )  +  0.5), 
floor (Xcorrect, Xrounded) ,  floor (Ycorrect ,Yrounded) , 
send(@path_xl,  selection,  Yrounded) , 
send (@path_yl,  selection,  Xrounded) , 
send (@mouse_click_path, append, point (XPointer, YPointer) ) . 

update__position_menus  (X,  Y) 

object (@new_x) ,  send(@new_x,  selection,  Y) , 
send(@new_y,  selection,  X),  fail, 
update josition^menus  (X,  Y) 

object (@trans_x) ,  send(@trans_x,  selection,  Y) , 
send{@trans_y,  selection,  X). 
update_position_menus  (X,  Y)  . 

update_transit_j)oint_menus (X, Y) 

object (@trans_x) ,  send(@trans_x,  selection,  Y) , 
send(@trans_y,  selection,  X) . 
update_transit_jpoint_menus  (X,  Y)  . 
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/*  If  a  successor  phase  has  not  been  specified  its  name  is  entered  here  */ 

/*  The- user  is  asked  to  enter  the  name  of  the  unspecified  phase.  In 
/*  order  for  code  to  be  generated  by  the  program,  the  named  phase  must 
/*  be  specified  later  */ 

get_unspecif  ied__phase  (_)  destroy_unspecif  ied_jphase_entry_objects,  fail. 

get_unspecified_jphase  (abort)  new (@okl, button  { 'OKS message  (@pro- 
log, abort_ok) ) ) , 
fail . 

get_unspecif ied_phase (complete)  : - 

new(@okl,button( 'OK' , message (@prolog, complete_ok) ) ) ,  fail. 
get_unspecifiedjphase (_)  new (@name_entry, dialog ( 'Unspecified  Phase  Name' ) ) , 
new (Label 1 , label ) , 

send (Labell, selection, 'Please  enter  the  intended  name  of  the  unspecified 
phase' ) , 

new (Label2 , label ) , 
send (Label2 , selection, 

'You  will  have  to  specify  this  phase  before  a  mission  can  be  generated'), 
new  (@name,  text__item( 'Name  ','')), 

send {@name_entry, append, Labell) ,  send (Label2 , below, Labell ) , 

send (©name, below, Label2 ) ,  send (©okl, be low, ©name) ,  send (@name_entry, open) . 

abort_ok  get (©name, selection, Name) ,  asserta (abort_successor (Name) ) , 

send (@name_entry, destroy) . 

complete_ok  get (©name, selection, Name) ,  asserta (complete_successor (Name) ) , 
send (©name_entry, destroy)  . 


/*  Callbacks  for  menus  and  pushbuttons  */ 

initial_choice  { 'Modify  Current  Mission')  \+phase  , 

invalid_option_report  (no__phases)  . 

initial__choice( 'Modify  Current  Mission')  \+phase_summary_list (X) , 
invalid_option_report (no_me_modify ) . 
initial_choice (_)  reset_to_main_menu ,  fail. 

ini tial_choice ( 'Create  Initialization  File')  ini tialization^menu . 
ini tial_choice ( 'Modify  Current  Mission')  phase_^eneration_menu . 
initial_choice(_)  abolish (phase/5) ,  abolish ( start_phase/l ) ,  fail. 

initial_choice ( 'Means  Ends  Generation')  destroy  phase  summary  menu, 
means_end_menu . 

ini tial_choice ( 'Phase  by  Phase  Generation')  destroy_phase__summary_j:nenu, 
display_phase_menu  ( [  [header,  ©phases,  @c_succ,  @a_succ]  ] ) , 
phase_genera  t i on_menu . 


File  :  meansend.pl 

Author  :  Neil  Rowe 

Modified  by:  Duane  Davis 

Project:  Pheonix  AUV  Strategic  Level  Mission  Generation  Expert  System 

Purpose:  This  file  contains  routines  for  conducting  means  ends  analysis. 
Operations,  preconditions,  and  postconditions  are  used  to 
calculate  a  path  from  a  start  state  to  a  desired  goal  state 
Operations,  pre  and  postconditions  have  been  created  for 
various  Pheonix  AUV  actions  and  capabilities. 

System  :  Quintus  Prolog  3.2  with  pwxpce  3.0  (Prowindows) 

Use  :  ai4/usr/work3/auv/strategic>  newprowin 
[mission_expert  ]  . 

?-  go. 

Date  :  29  April  96 
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means^ends (State, Goal, [] ,State)  difference (Goal, State, []) ,  1. 
means_ends(State,Goal,Oplist,Goalstate)  difference (Goal , State, D) , 
recommended (Dsub, Operator) , 
subset {Dsub,D) , 

precondition (Operator, Prelist, NotPrelist) , 
notprecondition (NotPrelist, State) , 
means_ends (State, Prelist, Preoplist, Prestate) , 
deletepostcondi tion (Operator, Deletepostlist) , 
deleteitems (Deletepostlist, Prestate, Prestate2 ) , 
addpostcondition (Operator, Addpost list ) , 
union (Addpost list , Prestate2 , Postlist) , 
means^ends (Postlist, Goal, Postoplist,Goalstate) , 

%  nl, write ('Completed  ->  '), write (Operator ), nl , 
append (Preoplist, [Operator! Postoplist] ,Oplist) . 
means_ends (State, Goal, Oplist,Goalstate)  difference (Goal , State, D) , 
recommended (Dsub, Operator) ,  subset (Dsub, D) , 
\+precondition(Operator, Prelist, NotPrelist) , 
write('Bug  found:  no  preconditions  found  for  operator  '), 
write (Operator) ,  nl,  !,  fail. 

means_ends (State, Goal , Oplist , Goalstate)  : -  difference (Goal , State, D) , 
recommended (Dsub, Operator) ,  subset (Dsub, D) , 

\+deletepostcondition (Operator, Deletepostlist ) , 

write ('Bug  found:  no  deletepostconditions  found  for  operator  '), 

write (Operator) ,  nl,  I,  fail. 

means_ends (State, Goal, Oplist, Goalstate)  difference (Goal, State, D) , 
recommended (Dsub, Operator) ,  subset (Dsub, D) , 

\+addpostcondi tion (Operator, Addpost list) , 

write ('Bug  found:  no  addpost conditions  found  for  operator  '), 
write (Operator) ,  nl,  i,  fail. 

difference ( [] ,S,  [] ) , 

dif ference( [P |G] ,S,G2)  singlemember (P, S) ,  !,  dif ference (G, S, G2 ) . 
difference( [PIG] ,S, [PIG2] )  dif ference (G, S, G2 ) . 

notprecondition ( [ ] , State) . 

notprecondition ( [XI Tl] , State)  \+singlemember (X, State) , 
notprecondition (Tl, State) . 


/*  Recommended  Operators  to  Achieve  Goal  States  */ 
recommended ( [ top_level_goal_done (X) ] ,  handle_top_level (X) ) . 
recommended ( [ [position(Xl, Yl, Zl) ,  transit (X2, Y2 , Z2) ] ] , 
hover (XI , Yl , Zl , X2 , Y2 , Z2 ) )  .  . 
recommended ( [position (X,Y,Z) ] ,  hover(X,Y,Z) ) . 
recommended ( [no_f ix_reqd] ,gps_fix) . 
recommended  ( [no_dive_wait_reqd]  ,dive_and_wait )  . 
recommended ( [ in_tube (X, Y, Z, Theta) ] , recover (X, Y, Z, Theta) ) . 


/*  Pre  and  Post  Conditions  for  Different  Phase  Types  */ 

/*  Top  Level  Goals,  One  Operator  With  Different  Operands  */ 

/*  to  define  different  goals  and  handle  them  accordingly  */ 

/*  Search  a  Position,  transit  via  a  location  prior  to  search  */ 
precondition (handle_top_level ( [ searched, XI ,Y1 , Zl, via, X2 ,Y2, Z2 ] ) , 

[  [position  {XI,  Y1,Z1)  ,  transit  (X2  ,Y2,  Z2 )  ]•,  no_f  ix__reqd, 
no_dive_wait__reqd] ,  [in_tube  {_,_,_)  ] )  . 
deletepostcondition(handle_top_level  ( [ searched, XI, Y1,Z1, via, X2,Y2,Z2] ) , 
[  [position(Xl,Yl,Zl)  ,  transit  (X2 ,  Y2 ,  Z2)  ] , no_f ix_reqd]  )  . 
addpostcondition (handle_top_level ( [ searched, XI, Yl, Zl, via, X2 ,Y2, Z2 ] ) , 
[top_level_goal_done( [searched, XI, Y1,Z1, via, X2,Y2,Z2] ) , 
position (XI, Y1,Z1) ,fix_reqd] ) . 
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/*  Search  a  Position,  transit  straight  to  the  position  */ 
precondition {handle_top_level ( [ searched, X, Y, Z] ) , 

[position (X, Y, Z) ,no_fix_reqd, no_dive_wait_reqd] , 

[in_tube  ] )  . 

deletepostcondition (handle_top_level ( [searched, X, Y, Z] ), 

[no_f ix_reqd] ) . 

addpostcondition(handle_top_level ( [searched, X,Y,Z] ) , 

[top_level_goal_done  ( [ searched, X,  Y,  Z] )  ,fix_reqd] )  . 

/*  Lower  Level  Goals,  accomplish  high  level  goals  preconditions  */ 

/*  Transit  to  point  (X1,Y1,Z1)  via  point  (X2,Y2,Z2)  and  hover)  */ 
precondition(hover(Xl,Yl,Zl,X2,  Y2,Z2) ,  [no_f ix_reqd, no_dive_wait_reqd]  , 
[in_tube  ] )  . 

deletepos tcondi tion (hover (X, Y, Z ,X2 , Y2 , Z2 ) , 

[ [position(01dXl,01dYl,01dZl) , transit(01dX2,01dY2,01dY3) ] , 
position (01dX,01dY,01dZ) ] )  . 

addpostcondition{hover(X,Y,Z,X2,Y2,Z2) , [ [position {X,Y, Z) , tran¬ 
sit  (X2,Y2,Z2) ] ] ) . 

/*  Transit  to  point  (X,Y,Z)  and  hover  */ 

precondition (hover {X,Y, Z) , [no_f ix_reqd, no_dive_wait_reqd] , [in_tube (_,_,_) ] ) . 
deletepos tcondi tion (hover (X, Y, Z ) , 

[ [position (01dXl,01dYl,01dZl) , transit  (bldX2, 01dY2,01dY3 ) ] , 
position (01dX,0ldY,01dZ) ] ) . 
addpostcondition (hover (X, Y, Z) , [position (X, Y, Z) ] ) . 

/*  Obtain  a  GPS  fix  */ 

precondition (gps_fix, [ ] , [in_tube ] ) . 
deletepostcondition (gps_f ix, [fix_reqd]) . 
addpostcondition {gps_fix, [no_fix_reqd] ) , 

/*  Dive  to  2  feet  and  wait  30  seconds  to  initialize  */ 
precondition(dive_and_wait, [] , [ in_tube (_,_,_) ] ) . 
deletepostcondition (dive_and_wait , [] ) . 
addpostcondition  {dive_and_wait,  [no_dive_wait_reqd] )  . 

/*  Recover  in  a  torpedo  tube  */ 

precondition(recover{X,y, Z, Theta) , [], [in_tube (_,_,_)] ) . 
deletepostcondition (recover (X,Y,Z, Theta) , [] ) . 
addpostcondition ( recover (X,Y,Z, Theta) , [in_tube(X,Y, Z, Theta) ] ) . 


File  :  mehelp.pl 

Author  :  Duane  Davis,  Brad  Leonhardt 

Project:  Pheonix  AUV  Strategic  Level  Mission  Generation  Expert  System 


Purpose ; 

This  file  contains  routines  for  specifying 
goal  state  conditions  for  an  AUV  mission, 
is  used  to  compute  a  series  of  phases  from 
goal  state. 

start  state  and 

Means  ends  analysis 
the  start  state  to  the 

System  : 

Quintus  Prolog  3.2  with  pwxpce  3.0  (Prowindows) 

Use 

ai4/usr/work3/auv/strategic>  newprowin 
?-  [mission_expert ] . 

?-  go. 

Date  : 

29  April  96 
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/*  means_Gnd_menu  brings  up  a  menu  that  the  user  uses  to 
/*  input  the  starting  point  of  the  vehicle,  the  recovery  point 
/*  of  the  vehicle,  and  any  points  that  the  user  wants  searched. 

/*  Means  ends  analysis  is  used  to  compute  a  series  of  phases 
/*  that  can  be  specified  to  accomplish  the  mission  */ 
means_end_menu  reset_to_main_jnGnu,  abolish (searchpoint/1) ,  fail. 

means_end_menu  new (@medialog, dialog ( 'Means  End  Help')), 

start_position{X,  Y,Z)  ,max_vehicle__depth (Zmax) , 
op_area  (Xmin,  Ymin,Xmax,ymax)  , 

new (Labell, label) ,  send {Labell , selection, 'Vehicle  Initial  Position') 
new (@startx, slider ( 'Launch  X  Position  ' , Xmin,Xmax,X) ) , 
new(@starty, slider ('Launch  Y  Position  ' , Ymin, Yraax, Y) ) , 
new(@startz, slider ('Launch  Depth  ' , 0 , Zmax, Z) ) , 

new(Label2, label) , send(Label2, selection, 'Vehicle  Recovery  Position' ) 
new (@endx, slider ( 'Recovery  X  Position  ' , Xmin,Xmax,X) ) , 
new(@endy,slider( 'Recovery  Y  Position  ' , Ymin, Ymax, Y) ) , 
new (@endz, slider ( 'Recovery  Depth  ' , 0, Zmax, Z) ) , 
new(Searchjpoint,button( 'Enter  Search  Point', 

message (@prolog,enter_point ) ) ) , 
new (Clear_points, button ( 'Clear  Search  Points', 

message  (@prolpg,  clear_searchjoints) ) )  , 
new (Generate, button ( 'Generate  Phase  Sequence', 

message (©Prolog, gen_phase_sequence) ) ) , 
new (Cancel, but ton ( 'Cancel' /message (©prolog, resGt_to_jnain_menu) ) } , 
new(Tube_recovery,button( 'Recover  In  Tube', 

message  (©prolog,  enter_tube„.data) ) ) , 
new (Clear_tube, button ( 'Cancel  Tube  Recovery', 

message (©Prolog, clear_tube_data) ) ) , 
send (©medialog, append, Labell) ,  send (©startx, below, Labell) , 
send (©starty, below, ©startx) ,  send (©startz, below, ©starty) , 
send (©startz , below, ©starty) ,  send {Label2 , below, ©startz ) , 
send (©endx, below, Label2) ,  send (@enc^, below, ©endx) , 
send (©endz, below, ©endy) ,  send (Search_point, below, ©endz) , 
send (Tube_rGcovery , right , Search_point ) , 
send  (Clearjoints,  below,  Search_point)  , 
send (Clear_tube, below, Tube_recovery ) , 

send (Generate, right, Tube_recovery) ,  send (Cancel, right, Clear_tube) , 
send ( ©medi al og , open ) . 


/*  gen_phase_sequence  marshals  all  of  the  data  from  the  means„end_menu 
/*  and  all  of  the  asserted  searchpoints  and  calls  the  means  end  analysi 
/*  routine  to  get  a  series  of  phases  to  accomplish  the  mission  */ 
gen_phase_sequence  abolish (start_state/l ) ,  abolish (goal_order/l) , 
get (©startx, selection, SX) , 
get (©starty, selection, SY) , 
get (©startz, selection, SZ) , 
get {©endx, selection, EX) , 
get (©endy, selection, EY) , 
get (©endz, selection, EZ) , 
marshal_goal_data (Sub_Goal_List) , 

append ( [position (EX, EY,EZ) ] , Sub_Goal_List, Goal_List) , 
asserta(start_state( [position(SX,SY, SZ) , no_f ix_reqd] ) ) , 
means_ends ( [position (SX,SY,SZ) ,no_fix_reqd] , Goal_List, Steps, _) , 
asserta(goal_order  (Goal_List) )  ,  reset_for_jne_solution, 
mesolution (Steps) ,  assGrt_phases (Steps) . 

/*  next_solution  finds  a  new  solution  using  means  ends  analysis  */ 
next_solution  start_state( [position (SX,SY,SZ)  I  L] ) , 
make__reordered_goal_list (Goal_List) , 

means_ends( [ [position (SX,SY,SZ)  I  Transit]  I  L] , Goal_List, Steps, _) , 
asserta(goal_order (Goal_List) ) ,  reset_for_me_solution, 
mesolution (Steps) ,  assert_phases (Steps) . 
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/*  enter_point:  is  used  to  enter  points  that  need  to  be  searched 
/*  searchpoint  facts  are  asserted  and  later  marshalled  for  use 
/*  in  the  means  end  analysis  */ 

enter_point  destroy__search_point__and_tube_objects, 
new{@pointdialog,dialog( 'Search  Point  Data')), 
new  (Searchjabel ,  label ) , 

send (Search^label, selection, 'Search  Point  Location'), 
make_x_y_depth_i terns , 

new  (Take,  but  ton  ( 'Store  Point' ,  message  (@prolog,  store__point) ) )  , 
new (Cancel, but ton ( 'Cancel' , 

message  (@prolog,destroy_search_point_and_tube__objects) ) )  , 
new (Trans_label , label ) , 

send(Trans_label, selection, 'Transit  Point  Location:'), 
get  (@path__xl, selection, X) ,  get  (@path_:yl,  selection,  Y)  , 
op__area  (Xmin,  Ymin,Xmax, Ymax) , 

new (@trans_x, slider ( 'X  Position  ' ,Xmin,Xmax, X) ) , 
new (@trans_y, slider ('Y  Position  ' , Ymin, Ymax, Y) ) , 
send(@pointdialog,  append, Search__label) , 

send (@new_x, below, Search_label) ,  send {@new_j/, below, @new_x) , 
send (@new_depth, below, @new_y) ,  send (Trans_label, below, @new_depth) , 
send {@trans_x, below, Trans_label) ,  send (@trans_y, below, @trans_x) , 
send (Take, below, @trans_y) , 

send (Cancel, right, Take) ,  send(@pointdialog, open) . 

store_point  : -  destroy _error_objects, 

invalid_point (Error) ,  ! ,  invalid_point_report (Error) . 
store_point  get (@new_x, selection, X) , 

get (@new_^, selection, Y) , 
get (@new_depth, selection, Z) , 
get (@trans_x, selection, TX) , 
get (@trans_y, selection, TY) , 
same(X,TX) ,same(Y,TY) , 

asserta (searchpoint (top_level_goal„done( [searched, X, Y, Z] ) ) ) , 
destroy_search_point_and_tube_objects. 
store_point  get (@new_x, selection, X) , 

get  (©new_j/,  selection,  Y) , 
get (©new_depth, selection, Z) , 
get (@trans_x, selection, TX) , 
get (@trans_y, selection, TY) , 

asserta (searchpoint (top_level_goal_done( [searched,X, Y, Z, via,TX,TY, Z] ) ) ) , 
destroy„search_point_and_tube__objects. 


/*  enter  tube  data  is  used  to  enter  the  location  of  */ 

/*  the  tube  into  which  the  auv  is  to  recover  */ 

enter_tube_data  :  -  destroy_search_point_and_tube_objects, 
new(@tubedialog,dialog{'Recovery  Tube  Data')), 
make_x_y_depth_i  terns , 
send {@new_x, name, 'Tube  X  Position  '), 
send (@new_y, name, 'Tube  Y  Position  '), 
send (@new_depth, name, 'Tube  Depth  ' ) , 
new(©heading,slider ( 'Tube  Entry  Heading' , 0 , 360 , 0 )) , 
new (Take , button ( 'Store  Data ' , message (©prolog, s tore_tube__data ) ) ) , 
new (Cancel, but ton ( 'Cancel' , 

message (©Prolog, destroy_search_point_and_tube_objects) ) ) , 
get  (@path_xl,  selection, X)  ,  get  (©path^l ,  selection,  Y)  , 
send (©tubedialog, append, @new_x) , 

send ( @new_y , bel ow , ©new_x ) ,  send ( @new_dep th , bel ow , @new_y ) , 
send (©heading, below, ©new_depth) , send (Take,  below, ©heading) , 
send (Cancel, right, Take) ,  send (©tubedialog, open) . 


/*  Store  Tube  Data  marshals  all  the  recovery  tube  location  */ 

/*  data  and  asserts  a  fact  with  the  tube  location  for  use  later  */ 
store_tube_data  : -  destroy_error_objects, 

invalidjpoint (Error) ,  ! ,  invalid_point_report (Error) . 
store_tube_data  get (@new_x, selection, X) , 
get (©new_y, selection, Y) , 
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get (@nGw_depth, selection, Z) , 
get (©heading, selection, Theta) , 

abolish (tube_data/4) ,  asserta (tube_data (X, Y, Z, Theta) ) , 
perpendicular_^oint (X, Y, Theta, Xapp roach, Yapp roach) , 
send {©endx, selection, Xapproach) , 
send {@endy, selection, Yapproach) , 
send (©endz, selection, Z)  , 
destroy_search_point_and_tube_objects . 


/*  mesolution  takes  the  sequence  of  phases  derived  through 
/*  means  ends  analysis  and  displays  them  in  a  dialog  box  */ 
mesolution! [LastStep] )  object (LastStep) ,  !, 

new (Cancel , button ( 'Cancel' ,message {©prolog, destroy _me_soln_objects) ) ) 
new (NextSoln, button ( 'Next  Solution' , message (©prolog, next__solution) ) ) , 
send(Cancel, below, LastStep) ,  send (NextSoln, right , Cancel) , 
send (©solutiondialog, open) . 
mesolution(Steps)  \+object (©solutiondialog) , 
abolish (me_steps/l) ,  asserta (me_steps (Steps) ), 
new(©solutiondialog,dialog{ 'Means  End  Solution')), 
new (Label, label) ,  send (Label, select ion, 'Computed  Phase  Sequence:'), 
new (Solutionlabel, label) , 

send(@solutiondialog, append, Label) ,  send (Solutionlabel, below, Label) , 
mesolution ( [Solutionlabel  1  Steps] ) . 
mesolution! [Previous,  Current  I  Rest])  me_step_label (Current , Label ) , 
new (Step_label, label) ,  send (Step__label, select! on, Label) , 
send (Step__label, below, Previous) ,  mesolution! [Step_labei  I  Rest]). 

make_reordered__goal_list  (Goal_List)  :  - 

goal_order(  [position (X,Y,Z)  I  Used__List] ) , 
same_members  (Sub_Goal_List  ,Used_List) , 

\+goal_order( (position (_,_,_)  I  Sub_Goal_List] ) , 
same!  [position (X,Y,Z)  I  Sub_Goal_List ] ,Goal_List ) . 
make_reordered_goal_list (Goal_List)  : - 

goal_order (Goal_List) ,  abolish (goal_order/l) . 


/*  me_step_label  matches  phase  commands  derived  through  means  ends 
/*  analysis  with  more  user  friendly  text  strings  */ 
me_step_label  (handle__top_level  (  [searched, X,  Y,  Z,  via, ) ,  Label )  :  - 
list_concat ([ 'Conduct  a  sonar  search  at  position  X=',X,'  Y=',Y, 

'  Depths' ,Z, ' . ' ] , Label) . 

me_step_label (handle_top_level ( [searched, X, Y, Z] ) , Label) 

list_concat( ['Conduct  a  sonar  search  at  position  X=',X, '  y-',Y, 

'  Depth=',Z, ' . '] ,Label) . 
me_step_label (sonar_search (X, Y, Z, _,_,_) , Label) 

list_concat{ ['Conduct  a  sonar  search  at  position  X=',X, '  Y=',Y, 

'  Depth=:' ,  Z,  ' . '  ]  , Label)  . 
me_step_label  (hover  (X,Y,  Z) , Label) 

list_concat( ['Hover  at  position  X=',X, '  Y=',Y, '  Depth=' , Z, ' . ' ] ,Label) 
me_step_label (hover (X,Y,Z, XT, yT,ZT) , Label) 

list_concat ( ['Transit  to  Position  X  =',XT, '  Y=',YT, '  Depth=',ZT, 

',  then  hover  at  position  X=',X, '  Y=',Y, '  Depth=' ,Z, ' , ' ] ,Label) , 
me_step_label (gps_fix, 'Obtain  a  GPS  fix'). 

me_step_label {dive_and_wait, 'Dive  to  2  feet  and  wait  30  seconds'). 
me_step_label (recover (X,Y, Z, Theta) , Label) 

list_concat ([ 'Recover  in  Tube  at  Position  X  =  ',X,  '  Y  =  ',Y, 

'  Depth  =  ',Z,'  heading  ', Theta,'  degrees' ], Label ) . 

/*  Use  the  Results  from  Means  Ends  Analysis  to  assert  phase  facts  */ 
assertjhases  ( [  ] )  !. 

assert_phases ( [dive_and_wait  I  Rest])  next_phase (Current ) , 
get_next_phase ( [dive_and_wait  I  Rest] ,Next) , 

assertz (phase (Current, 'Depth  Change' , [2,100] ,Next, 'mission_abort ' ) ) , 
get_next  jhase  (Rest,  Next2)  , 

assertz {phase(Next, 'Wait ' ,  [30] ,Next2 ,Next2 ) ) , 
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assert_phases (Rest) . 

assert_phases( [hover (X,Y,Z)  I  Rest])  next_phase (Current) , 

get_next_phase (Rest, Next) ,  get_fail_next (Current, Rest, FailNext) , 
assertz (phase (Current, 'Hover', [X,Y, Z, 0, 500] , Next, FailNext) ) , 
extend_ine_route(X, Y) ,  assert_phases (Rest) . 
assert_phases ( [hover (X,Y,Z, XT, YT,ZT)  I  Rest])  next_phase (Current) , 
CurrentPlusl  is  Current  +  1, 

get_next_phase([hover(X,Y,Z,XT,YT,ZT)  I  Rest], Next), 
assertz (phase (Current, 'Transit', [XT,YT,ZT,500] , Next, CurrentPlusl) ) , 
extend_ine_route(XT,YT) ,  assert_phases ( [hover {X,Y,Z)  1  Rest]). 
assert_phases ( [gps_fix  |  Rest])  next_phase (Current ) , 
get_next_phase (Rest, Next) , 

get_f ail_next ( Current , Rest , Fai INext ) , 

assertz (phase (Current, 'GPS  Fix' , [150] , Next, FailNext) ) , 

assert_phases (Rest) . 

assert_phases ( [handle_top_level ( [searched, X,Y,Z, via, _,_,_] )  I  Rest]) 
next_phase (Current) , 

ge t_next_phase ( Res t , Next ) ,  get_f ai l_next (Current , Res t , FailNext ) , 
assertz (phase (Current, 'Rotate  Sonar  Search' , [X, Y,Z, 250] , Next, FailNext) ) , 
assert_phases(Rest) . 

assert_phases((handle_top_level([searched,X,Y,Z])  I  Rest]) 
next_jphase (Current)  , 

get_next_phase( Rest, Next) ,  get_fail_next (Current, Rest, FailNext) , 
assertz (phase (Current, 'Rotate  Sonar  Search' , [X,Y, 2,250] , Next, Fai INext) ) , 
assert_phases (Rest) . 

assert_phases ( [recover (X,Y, Z, Theta)  I  Rest]) 

next_phase (Current) ,  get_next_phase (Rest, Next) , 

assertz (phase (Current, 'Recover  in  Tube' , [X,Y, Z, Theta, 500] , 

Next, 'inission_abort' ) ) ,  assert_phases (Rest) . 


/*  next  phase  in  list  that  is  either  a  transit  or  hover  */ 
get_next_transit_phase ( [] , 'mission_abort' )  :=  ! . 

get_next_transit_phase( [First  I  Rest],l)  same(First,hover(_,_,_,_,_,_) ) , 
get_next_transit_phase( [First  I  Rest],l)  same(First,hover(_,_,_) ) ,  !. 
get_next_transit_phase ( [First  |  Rest] ,X)  get_next_transic_phase(Rest,Y) , 
nuinber(Y),  X  is  Y  +  1,  !. 
get_next_transit_phase  (_,  'iTiission_abort' )  . 

/*  if  a  phase  fails,  the  followon  phase  will  be  the  next  transit  */ 
get_fail_next (Current, Rest, FailNext) 

get_next_transit_phase (Rest, JumpOnFail) , 
number (JumpOnFail ) ,  FailNext  is  Current  +  JumpOnFail. 
get_fail_next (Current, Rest, 'mission_abort') . 

/*  next  phase  in  the  list  */ 
get_next_phase ( [] , 'mission_complete' ) . 

get_next_phase(L,Next)  retract (next_phase (Current) ) ,  Next  is  Current  +  1 
asserta (next_phase (Next) ) . 

extend_me_route (_,_)  \+object (@me_route) ,  new (@me_route, path)  , 
send (8me_route,pen, 2 ) , 

get(@startx, selection, X) ,  get (Sstarty, selection, Y) , 

x_zero (X_zero) ,  x_scale (X_scale) ,  y_zero (Y_zero) ,  y_scale(Y_scale) , 

XDraw  is  ( (Y  *  X_scale)  +  x_zero  -  0.5  *  X_scale) ,  floor (XDraw,XDraw2 ) , 

YDraw  is  ( (X  *  Y_scale)  +  Y_zero  -  0.5  *  Y_scale) ,  floor (YDraw, YDraw2 ) , 

send(eme_route, append, point (XDraw2,YDraw2) ) , 
send (Qpicture, display, @me_route) ,  fail, 
ext  end_me_r ou  t  e ( X , Y )  : - 

x_zero (X_zero) ,  x_scale(X_scale) ,  y_zero (Y_zero) ,  y_scale (Y_scale) , 

XDraw  is  ( (Y  *  X_scale)  +  X_zero  -  0.5  *  X_scale) ,  floor (XDraw, XDraw2 ) , 

YDraw  is  ( (X  *  Y_scale)  +  Y_zero  -  0.5  *  Y_scale) ,  floor (YDraw, YDraw2 ) , 

send (eme_route, append, point (XDraw2,YDraw2) ) . 

marshal_goal_data(Data_list)  tube_data (X,Y, Z, Theta ) , 

bagof_searchpoints (Point_list) , 

append( [in_tube(X, Y, Z, Theta) ] , Point_list,Data_list) . 
marshal_goal_data (Data_list)  bagof_searchpoints (Data_list) . 
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bagof_searchpoints(Point_List)  bagof {X, searchpoint (X) ,  Point_List ) ,  ! . 
bagof_searchpoints ( [ ] ) . 


%  File  : 
% 

%  Author  : 
% 

%  Project: 
% 

%  Purpose: 
% 


% 

% 

%  System 
% 

% 

% 

% 


Use 


generator.pl 

Duane  Davis,  Brad  Leonhardt 

Pheonix  AUV  Strategic  Level  Mission  Generation  Expert  System 

This  file  contains  routines  for  asserting  phase  facts  (used  to 
generate  the  commandos t rings  file) ,  creating  the  command_st rings 
file,  and  generating  C++  and/or  Prolog  Strategic  Level  Code 

Quintus  Prolog  3.2  with  pwxpce  3.0  (Prowindows) 

ai4/usr/work3/auv/strategic>  newprowin 
?“  [mission_expert)  . 

?-  go. 


%  Date  :  29  April  96 


/*  Callback  when  mission  specification  complete  */ 

spec_complete  :-  \+phase  ,  invalid_option_report  (no^phases)  . 

spec_complete  :-  destroy__error_objects,  \+start_phase  (X) ,  >, 
new{@first_phase, dialog! 'Start  Phase' ) ) , 

new (Phase_menu, menu ( 'Select  Desired  First  Phase:  ',choice)), 
send{Phase_menu, layout, vertical) , 

phase_list (Phase_list) ,  construct_menu (Phasejmenu, Phase_list , assert_start ) , 
send (@first_phase, append, Phase_menu) ,  send {@ firs t_phase, open) . 
spec_complete  :-  parse_mission. 

assert_start (Start)  :-  abolish {start_phase/l ) ,  asserta (start_phase (Start) ) , 
send(@first_phase, destroy) ,  parse_mission . 


/*  assert_phase  asserts  a  fact  of  the  form:  */ 

/*  phase (Name,  Type,  Parameter_list ,  Complete_successor,  Abort_successor)  */ 
/*  for  each  phase  specified  by  the  user  */ 

assertjhase  invalid_phase  (Error) ,  *,  invalid_phase_report  (Error)  . 
assert_phase  abolish (entry _mode/l) ,  get (@phase_name, selection, Phase_name) , 

retract  (phase  (Phase_name, ) , 

'retract (phasG_list (PList) ) ,  delete (Phase_name, PList,NewPList) , 
asserta (phase_list (NewPList) ) ,  fail . 
assertjhase  cur rent_phase  (Phase) , 

phase_parameters  (Phase,  Parameters) ,  get  (@phase_name,  selection,  Phase_name) , 
complete^successor (CSuccessor) , abort_successor  (ASuccessor) , 
asserta (phase (Phase^name, Phase, Parameters, CSuccessor, ASuccessor) ) , 
retract  (phase_list  (Phase_list)  ) ,  asserta  (phase__list  (  [Phase_name  1 
Phase_list] ) )  , 

pad_to_30  (Phase_name,Name_string) ,  pad_to_30  (CSuccessor, CSucc_string) , 
pad_to_30  (ASuccessor,  ASucc_string)  , 
phase_summary_list-{oidPList)  , 

delete!  [_,Name_string,_,_]  ,01dPList, 0ldPList2)  , 

append (01 dPList2, [ [ Phase, Name_string,CSucc_string,ASucc_string] ] , NewPList) , 
display_phase_menu  (NewPList)  , 
destroy ^phase^entry^objects . 


/*  phase_parameters  marshals  the  parameters  required  for  different  phase  types 
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/*  into  a  list  for  inclusion  in  the  phase  facts  asserted  by  assert_phase  */ 
phase_parameters( 'Depth  Change' , [Depth,  Time_out] ) 
get (@new_depth, selection, Depth) , 
get  {@tiine_out,  selection,  Time_out)  . 
phase_paraineters ( 'Course  Change' , [Heading,  Time_out]) 
get {©heading, selection, Heading) , 
get  (©tiine_out,  selection, Tiine_out)  . 
phasejparaineters{ 'Wait',  [Tiine_out]) 
get  (@tiine_out,  selection, Tiine_out)  . 
phase_parameters( 'Transit', [X,  Y,  Depth,  Time-out]) 
get (©new_x, selection, X) , 
get  (@new^,selection,  Y) , 
get (©new_depth, selection, Depth) , 
get  (©time_out,  selection,  Tiine_out)  . 
phase_parameters( 'Hover' , [X,  Y,  Depth,  Heading,  Time-out]) 
get (©new_x, selection, X) , 
get (@new_y,selection, Y) , 
get (©heading, selection, Heading) , 
ge  t { ©  new_dep  th , s  el ec  t i on , Dep  th ) , 
get  (@time__out,  selection,  Tiine_out )  , 
phasejarameters  ( 'GPS  Fix' ,  [Tiine_out] ) 
get (@time_out, selection, Time_out ) . 
phase_^arameters( 'Rotate  Sonar  Search', [X,  Y,  Depth,  Time_out]} 
get (©new_x, selection, X) , 
get (©new_y, selection, Y) , 
get  (@new__depth,  selection, Depth) , 
get (©time_out, selection, Time_out) . 
phasejparametersi 'Rotate  AUV  Search' , [X,  Y,  Depth,  Time_out]) 
get (@new_x, selection, X) , 
get (©new_y, selection, Y) , 
get (@new_depth, selection, Depth) , 
get  (©time-out,  selection,  Tiine_out)  . 

phase_parameters( 'Recover  in  Tube',[X,  Y,  Depth,  Heading,  Time_out] ) 
get (@new_x, selection, X) , 
get (@new_y, selection, Y) , 
get (©heading, selection, Heading) , 
get (©new_depth, selection, Depth) , 
get  (@tiine_out,  selection,  Time_out)  . 


/*  Check  mission  for  errors  and  generate  code  if  mission  valid*/ 
parse_mission  destroy_error_objects,  fail. 

parse_mission  mission_error (Phase,  loop) ,  !, 

invalid__mission_report  (Phase,  loop)  . 
parse_mission  phase (Phase, , 

setof (Error, mission_error (Phase, Error) ,Error_list) ,  1 , 
destroy_error_objects,  phase_error_report ( Phase, Error_list ) , 
mission_error{_, unreachable) ,  abolish (start_phase/l ) . 
parse_mission  tell (commandos t rings ) ,  fail. 

parse_mission  get {@file_name, selection, File_name) , 

start_phase(Start_phase) , 

list_concat ( [ 'file_name  ',File_name, '  ' , Start_phase] , Command) , 
write (Command) ,nl,  fail. 

parse_mission  phase (Phase, ,  genera te_code (Phase) ,  fail. 

parse_jnission  told,  code_dialog. 


/*  Generate  code  writes  a  series  of  strings  to  a  file  called  "mission_strings" 
/*  These  strings  are  used  by  the  C  program  to  generate  code  in  whatever 
/*  language  is  required.  The  current  version  generates  Prolog  code.  */ 
generate_code (Phase)  : - 

phase (Phase, 'Recover  in  Tube', [X,Y, Depth, Theta, Time_out] ,CSucc,ASucc) , 
list^concat ( [ ' tube_recovery  ' , Phase, '  ' , CSucc, '  ' , 'withdraw  ' , 

Time_out,'  ',X, '  ',Y, '  ', Depth,'  Theta] ,Commandl) , 
wr i t  e ( Commandl ) , nl , 
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approach_point (X, Y, Theta, Xout, Yout) , 

list_concat ( [ ' hoverpoint  withdraw  ",ASucc,'  SASucc,'  250  \ 

Xout,  '  ',Yout,'  '/Depth,'  ',  Theta  ],  Cominand2  )  , 
write { Coinmand2 )  , nl . 

generate_codG (Phase)  phase { Phase, ' Depth 

Change' ,  [Depth, Tiine__out ] , CSucc,  ASucc)  , 

list^concat ( [ 'depth_change  ', Phase,'  ', CSucc,'  ', ASucc,'  ', 

Time_out, '  ' , Depth] /Command) , 
write (Command) ,nl. 

generat encode (Phase)  phase (Phase, ' Course  Change' , [Head¬ 

ing, Time_out]  /CSucc, ASucc) , 

list_concat ([ 'course  '/Phase,'  ', CSucc,'  ', ASucc,'  ', 

Time_out , '  ' , Heading] , Command) , 
write (Command) , nl . 

generate_code (Phase)  phase(Phase, 'Wait', [Time_out] , CSucc, ASucc) , 

list_concat ( [ 'wait  ', Phase,'  ', CSucc,'  ', ASucc,'  ' ,Time_out] /Command) , 
write (Command) , nl . 
generate_code (Phase) 

phase (Phase, 'Transit', [X,Y, Depth, Time_out] , CSucc, ASucc ) , 
list_concat ([ 'waypoint  ', Phase,'  ', CSucc,'  ', ASucc,'  ', 

Time_out, '  ',X, '  ',Y, '  ', Depth] /Command) , 
wr i te ( Command) , nl . 

generate^code (Phase)  phase (Phase, 'Hover' , [X,Y, Depth, Head¬ 

ing,  Time_out]  /CSucc, ASucc) , 

list_concat ( [ 'hoverpoint  ', Phase,'  ', CSucc,'  '/ASucc,'  ', 

Time-out,'  ',X, '  ',Y, '  ', Depth, '  ', Heading] , Command) , 
write (Command) , nl . 

generate_code (Phase)  phase (Phase, ' GPS  Fix' , [Time_out] , CSucc, ASucc) , 
list_concat ( [ 'get_gps_fix  ', Phase,'  ', CSucc,'  ', ASucc,'  ',Time_out], 
Command) ,  write (Command) , nl . 
generate_code ( Phase )  : - 

phase (Phase, 'Rotate  Sonar  Search' , [X, Y, Depth, Time_out] , CSucc, ASucc) , 
list_concat ( [ ' rotate_sonar_search  ', Phase, '  ', CSucc,'  ', ASucc,'  ', 

Time_out, '  ',X, '  ',Y,'  ', Depth,'  '], Command) , 
write (Command) , nl . 

generate_code (Phase)  phase (Phase, ' Rotate  AUV 
Search' ,  [X,  Y, Depth, Time_out ] ,. CSucc,  ASucc)  , 

list_concat ( [ ' sonar_search  ', Phase,'  ', CSucc,'  ', ASucc,'  ', 

Time_out, '  '/X,'  '/Y,'  '/Depth,'  Command), 
write (Command) , nl . 

code_dialog  :-  destroy_code_selection_objects,  fail. 
code_dialog  new (@code__dialog, dialog ( 'Language' ) ) , 
new (Label , label ) , 

send (Label , selection, ' Parsing  Complete,  Choose  Desired  Output  Language:'), 

new(Cpp,button('C++',message(@prolog, language, 'C++') ) ) , 

new( Prolog, button ( 'Prolog' , message (@pro log, language, 'Prolog' ) ) ) , 

new  (Done,  button  ( 'Cancel ' , message  (Oprolog,  destroy_code_selection_objects). )  ) , 

send (@code_dialog, append, Label) ,  send (Cpp, below, Label) , 

send (Prolog, right, Cpp) ,  send (Done, right, Prolog) ,  send(©code_dialog, open) . 

language ( 'C++ ' )  :-  unix(shell ( 'mission_cpp' ) ) ,  destroy_codG_selection_objects . 
language  ( 'Prolog' )■  :-  unix (shell  ( 'mission^)!' ))  , 
destroy_code_selection_objects . 


%  File  :  initialization.pl 
% 

%  Author  :  Duane  Davis,  Brad  Leonhardt 
% 

%  Project:  Pheonix  AUV  Strategic  Level  Mission  Generation  Expert  System 


%  Purpose:  This  file  contains  routines  for  specifying  and  creating  the 
%  initialization  file  for  an  AUV  mission.  The  initialization 

%  file  contains  mission  specific  information  not  required  at  the 

%  strategic  level  such  as  dive  tracker  locations,  initial  vehicle 
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oV>  o'P  oV  oV  oV>  oV>  oV>  oV  oV 


posture,  and  gyro  error. 

System  :  Quintus  Prolog  3.2  with  pwxpce  3.0  (Prowindows) 

Use  :  ai4/usr/work3/auv/strategic>  newprowin 
?-  [mission_expert] . 
go. 

Date  ;  29  April  96 


initialization_menu  reset_to_main__menu,  \+object (©initialize_dialog) , 
start_position (X, Y, Z) , 

max_vehicle_depth(Zmax) ,  op_area (Xmin, Ymin, Xmax, Ymax) , 
new (@ini tialize_dialog , dialog ( ' Initialization  Parameters ' } ) , 
new ( Posture_label ,  label ) , 

send (Posture_label, selection, ' Initial  Posture' ) , 
new(@posture_x, slider { 'X  ' ,Xmin,Xmax,X) ) , 
new(@posture_y, slider { 'Y  ' ,Xmin,Xmax, Y) ) , 
new (@posture_z, slider ( 'Depth  ' , 0, Zmax, Z) ) , 
n€w{@posture_phi,slider ( 'Roll  Angle  ',0,360,0)), 
new(@posture_theta,slider('Pitch  Angle  ',0,360,0)), 
new(@posture_psi,slider( 'Heading  ' ,0,360,0) ) , 
new{Blankl, label ) , 
new (DTl_label, label) , 

send {DTl_label, selection, 'Dive  Tracker  Unit  1  Location'), 

new(@dtlx, slider ( 'X  ' ,Xmin,Xmax, 0) ) , 

new(@dtly, slider ('Y  ' , Ymin, Ymax, 0) ) , 

new (@dtlz, slider ('Depth  ' , 0, Zmax, 2 ) ) ,  ^ 

new (Blank2, label) , 

new (DT2_label, label) , 

send(DT2_label, selection, 'Dive  Tracker  Unit  2  Location'), 

new(@dt2x, slider ('X  ' ,Xmin,Xmax, 0) ) , 

new(©dt2y, slider ( 'Y  ' , Ymin, Ymax, 0) ) / 

new(©dt2z, slider ('Depth  ',0, Zmax, 2) ) , 

new (Blanks, label) , 

new(@gyro_error,slider ( 'Gyro  Error  ',0,360,0)), 

new (Done, button ( 'Done' , message (©prolog,make_init_f  ile) ) ) , 

new(Cancel,button('Cancel',message(©prolog,reset_to_main_menu) ) ) , 

send(iainitialize_dialog,  append,  Posture__label )  , 

send (@posture_x, below, Posture_label ) , 

send  (@posture__y,  below,  @posture_x) , 

send (@posture_z , below, @posture_y ) , 

send  (@posture_phi,  below,  @posture__z) , 

send (@posture_theta, below, @posture_phi ) , 

send (@posture_psi ,  below,  @posture__theta ) , 

send (Blankl , below, @posture_psi ) , 

send(DTl_label,below,Blankl) , 

send (@dtlx, below, DTl_label) , 

send (@dtly, below, ©dt lx) , 

send (@dtlz, below, @dtly ) , 

send (Blanks, below, @dtlz) , 

send ( DT2_1 abel , bel ow , ©dt 1 z ) , 

send (©dt2x, below, DT2_label) , 

send (@dt2y, below, @dt2x) , 

send (©dtSz, below, @dt2y) , 

send (Blanks, below, ©dt2z) , 

send(@gyro_error, below, Blanks) , 

send (Done, below, ©gyro_error) ,  send (Cancel, right, Done) , 
send (@initialize_dialog, open) . 


make_init_f ile  tell ( 'initialization. script ') , 
get (@posture_x, selection, PX) , 
get (@posture_y, selection, PY) , 
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get (@posture_z , selection, PZ) , 
get (@posture_phi , selection, PPhi) , 
get (@posture_theta, selection, PTheta) , 
get {@posture_psi, selection, PPsi) , 

list_concat( ['POSTURE  ' , PX, '  ',PY, '  ',PZ,'  ',PPhi,'  ', PTheta,'  ',PPsij, 
Posture) , 

list_concat{ ['HEADING  ', PPsi ], Heading) , 
get (@dtlx, selection, DtlX) , 
get (@dtly, selection, DtlY) , 
get (@dtlz, selection, DtlZ) , 

list_concat( ['DIVE-TRACKERl  ',DtlX, '  ',DtlY,'  ' , DtlZ] ,DT1) , 
get (edt2x, selection, Dt2X) , 
get (@dt2y, selection, Dt2Y) , 
get {©dt2z, selection, Dt2Z) , 

list_concat( ['DIVE-TRACKER2  ',Dt2X,'  ',Dt2Y,'  ' , Dt2Z] , DT2 ) , 

get ( ©gyro_error , selection, G_error) , 

list__concat  {  ['GYRO-ERROR  '  ,G_error  ] ,  Gyro_Error) , 

write (Posture) ,  nl,nl, 

write (Heading) ,  nl,nl, 

write (DTI),  nl,nl, 

write(DT2),  nl,nl, 

write ( Gy ro_Error) ,  nl,nl, 

write ('THRUSTERS -ON'),  nl,nl, 

told,  reset_to_inain_menu . 


%  File  : 
% 

%  Author  : 
% 

%  Project: 
% 

%  Purpose: 
% 

% 

% 

%  System  : 
% 

%  Use  : 
% 

% 

% 

%  Date  : 


errors.pl 

Duane  Davis,  Brad  Leonhardt 

Pheonix  AUV  Strategic  Level  Mission  Generation  Expert  System 

This  file  contains  rules  for  determining  when  invalid  phases, 
missions,  or  operations  are  attempted,  and  routines  for 
displaying  error  messages  as  appropriate. 

Quintus  Prolog  3.2  with  pwxpce  3.0  (Prowindows) 

ai4/usr/work3/auv/strategic>  newprowin 
?-  [mission_expert] . 

?-  go. 

29  April  96 


/*  Rules  for  determining  when  a  specified  phase  is  invalid  */ 
invalid_phase (no^name)  get {@phase__name, selection,  "  ) . 
invalid_phase  (duplicate_phase)  \+entry_mode (modify)  , 
get  (@phase_name,  selection.  Name) ,  phase  (Name, . 
invalid_phase (no_successor)  \+abort_successor (X) . 

invalid_j)hase  (no_successor)  :  abort_successor  (  "  )  . 
invalid_phase (no_successor)  \+complete_successor (X) . 

invalid_phase (no__successor)  : -  complete_successor ( ' ' ) . 
invalid_phase (non^number)  : -  object (@time_out) ,get (@time_out , selec¬ 
tion.  String)  , 

\-f St ring_to_num (String, _)  . 

inval id^phase (non_number)  : -  object (@new_x) , get (@new_x, selection, String ) , 
\+string_to_num (String, _)  . 

inval id__phase  (non_number )  object  (©new  y)  , get  (©new__y,  selection.  String)  , 

\+string__to_num (String, _)  . 

invalid_phase (non_number)  : -  object (©new_depth) ,get (©new_depth, selec¬ 
tion,  String) , 

\+string_to_num (String, _)  . 

invalid_jphase (non_number)  : -  object (©heading) , get (©heading, selection, String) , 


277 


\  +  string_to__num( string, _)  . 

invalid_phase  (too^deep)  object  (@new__depth) ,  max^vehi cl e_depth (Deepest)  , 
get (@new_depth, selection, Depth) ,  Depth  >  Deepest. 
invalid_phase  (bad_heading)  object  ((Sheading) , 

get  ((Sheading,  selection.  Heading)  , 

(Heading  >  360;  Heading  <  0) . 

invalid^phase (bottoin_hit)  object (@new_depth) ,  max_area_depth (Bottom) , 

get (@new_depth, selection, Depth) ,  Depth  >  Bottom. 
invalid__phase  (too_s hallow)  object  (@new_depth)  , 

get (@new_depth, selection, Depth) ,  Depth  <0. 
invalid_phase  (out_of_area)  object  (@new_x)  ,  get  ((Snew_x,  selection, X)  , 
op_area(Xl,_,X2,_) ,  (X  <  XI;  X  >  X2) . 
invalid_phase  (out_of_area)  object  (@new_y) ,  get  {(inew^y,  selection,  Y)  , 
op_area(_,Yl,_,Y2) ,  (Y  <  Yl;  Y  >  Y2) . 
invalid_phase (bottom_hit )  object (@new_depth) , 

get  ((inGw__depth,  selection, Depth) , 

object  {(anew_x)  ,  get  (@new_x,  selection,  X)  ,  object  (@new_j/ )  , 
get  ((inew_y ,  selection,  Y)  , 

area_depth(Xl,Yl,X2,Y2,Area_depth)  ,  Y  >=  Yl,  Y  =<  Y2 ,  X  >=  XI,  X  =<  X2, 
Area__depth  <  Depth. 

invalid_phase  (not_mission_complete)  currentjhase  ( 'Recover  in  Tube'), 
\+complete_successor ( 'mission_complete' ) . 

/*  Rules  for  finding  errors  in  a  specified  mission  */ 

mission_error (Phase, loop)  phase (Phase, ,  reachable (Phase, Phase) . 

mission__error  (_,no_file)  get  ((if ile_name,  selection,  ")  . 

mission_error  (_,no_start)  \+start_phase  (_)  . 

mission_error  (_,no_complete)  \+phase  'mi ssion_complete' )  , 

\+phase  (_,_,_,  'mission_complete' ,_) . 
mission__error  (Phase,  dangle)  :  - 

phase (Phase, Successor, _) ,  \+phase (Successor, , 

\+same (Successor, 'mission_abort' ) ,  \+same (Successor, 'mission^complete' ) . 
mission_error (Phase, dangle)  phase (Phase, Successor) , 

\+phase (Successor, , 

\+same (Successor, 'mission_abort' ) ,  \+same (Successor, 'mission_complete' ) . 
mission_error (Phase, unreachable)  start_phase (Start ) ,  phase (Phase, , 
\+same (Start , Phase) ,  \+reachable (Start , Phase) . 
mission_error (Phase, hover_for_recovery)  : - 
phase (Phase, 'Recover  in  Tube , 

phase ( Predecesor, Type, Phase, _) ,  \+same(Type, 'Hover') . 
mission_error (Phase, hover_for_recovery)  : - 
phase (Phase, 'Recover  in  Tube' , 

phase (Predecesor, Type, Phase) ,  \+same(Type, 'Hover') . 
mission_error (Phase, long_recovery)  : - 

phase  (Phase, 'Recover  in  Tube' ,  [XI,  Yl, , 

phase ( Predecessor, _, [X2 , Y2 , Phase, _) ,  distance (XI, Yl ,X2, Y2 ,Dist) , 
Dist  >  25.0. 

mission_error (Phase, long_recovery) 

phase (Phase, 'Recover  in  Tube' ,  [XI, Yl, , 

phase ( Predecessor, [X2, Y2 Phase) ,  distance (XI, Y1,X2, Y2 , Dist) , 
Dist  >  25.0. 

mission__error  (Phase,  short_recovery)  :  - 

phase ( Phase, 'Recover  in  Tube' ,  [XI,  Yl, , 

phase ( Predecessor, [X2, Y2 , Phase, _)  ,  distance (XI, Yl ,X2, Y2 , Dist) , 
Dist  <  5.0. 

mission_error (Phase, short_recovery) 

phase  (Phase, 'Recover  in  Tube' ,  [XI,  Yl, , 

phase  (Predecessor, _,  [X2,Y2 ,_,  Phase)  ,  distance  (XI,  Y1,X2,Y2 , Dist) , 
Dist  <  5.0. 

mission_error  (Phase,  displaced_search) 

phase (Phase, 'Rotate  Sonar  Search' , [X,Y, Z, , 
phase (_, Type, Parameters, Phase, _) , 

(\+same(TVpe, 'Hover' ) ;  \+same (Parameters, [X, Y, Z,_,_] ) ) . 
mission_error  ( Phase,  dispiaced_,search)  :  - 

phase (Phase, 'Rotate  Sonar  Search' , [X, Y, Z, _],_,_)  , 
phase Type, Parameters, Phase) , 

(\+same  (Type, 'Hover ') ;  \+same  (Parameters,  [X,  Y,  Z, _,__]))  . 
mission_error (Phase, displaced_search)  : - 
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phase (Phase, 'Rotate  Sonar  Search' , [X, Y, Z, , 

\+phase{__,  'Hover' Phase, _)  ,  \+phasG(_,  'Hover Phase)  . 
inission_error (Phase, displaced_search)  : - 

phase  (Phase, 'Rotate  AUV  Search' ,  [X,Y,  Z, , 
phase  L,'iype,  Parameters,  Phase, _)  , 

(\+same(Type, 'Hover' ) ;  \+same (Parameters, [X,Y,Z,_,_] ) ) . 
mission_error (Phase, displaced_search)  : ^ 

phase (Phase, 'Rotate  AUV  Search' , [X,Y, Z, , 
phase  (_,  Type ,  Parameters , Phase ) , 

(\+same(TVpe, 'Hover' ) ;  \+same (Parameters, [X,Y,Z,_,_] ) ) . 
mission_error (Phase, displaced_search)  : - 

phase  (Phase, 'Rotate  AUV  Search' ,  [X,  Y,  Z, , 

\+phase(_,  'Hover' Phase, _)  ,  \+phase(_,  'Hover Phase)  . 


/*  Rules  for  determining  when  a  specified  search  or  transit  point  is  invalid  */ 
/*  Search  and  transit  points  are  generated  by  the  means  ends  facility  */ 

invalid_point (non_number)  object (@new_x) , get (@new_x, selection, String) , 

\  +  string_to__num (String, _)  . 

invalid_jpoint (non_number)  : ~  object (@new_y) ,get (@new_y, selection, String) , 
\+string_to_num(String,__)  . 
invalid_point (non_number)  object (@new_depth) , 

get  (@new__depth,  selection.  String)  , 

\  +  string__to_num (String, _)  . 

invalid_point (non_number)  object (@trans_x) ,get (Qtrans^x, selection, String) , 

\+string_to_num (String, _) . 

invalid_point  (non^number)  :  -  object  (@trans_y)  ,get  ((atrans_y ,  selection,  String)  , 

\  +  string_to_num (String, _)  . 

invalid__point  (too_deep)  object  (@new_depth) ,  max_vehicle_depth  (Deepest ) ', 
get  (@new_depth,  selection.  Depth) ,  Depth  >  Deepest. 
invalid_point  (bottom__hit)  object  (@new_depth)  ,  max__area_depth (Bottom)  , 

get (@new_depth, selection, Depth) Depth  >  Bottom. 
invalid_point (too_shallow)  object (@new_depth) , 

get (@new_depth, selection, Depth) ,  Depth  <  0. 
invalid_jpoint  (out_of_area)  object  (@new_x) ,  get  (@new__x,  selection, X) , 
op_area(Xl,_,X2,_) ,  (X  <  XI;  X  >  X2)  . 
invalid_point (out_of_area)  object (@new_y) ,  get (@new_7, selection, Y) , 

op_area{_,Yl,_,Y2) ,  (Y  <  Yl;  Y  >  Y2) . 
invalid_point (bottom_hit)  object (@new_depth) , 

get (@new_depth, selection, Depth) , 

object (@new_x) ,  get (@new_x, selection, X) ,  object (@new_y) , 
get (@new_y, selection, Y) , 

area_depth(Xl,Yl,X2,Y2,Area_depth)  ,  Y  >=  Yl,  Y  =<  Y2,  X  >=  XI,  X  =<  X2, 
Area_depth  <  Depth. 

invalid_point (out_of_area)  object (@trans_x) ,  get (@trans_x, selection, X) , 

op_afea(Xl,_,X2,_)  ,  (X  <  XI;  X  >  X2)  . 

invalid_point (out_of_area)  object (@trans_y) ,  get (@trans_y, selection, Y) , 

op_area(_,yi,_,Y2) ,  (Y  <  Yl;  Y  >  Y2) . 
invalid_point (bottom_hit)  object (@new_depth) , 

get (@new_depth, selection, Depth) , 

object  {@trans_x) ,  get  (@trans_x,  selection, X) ,  object  { (it rans_y) , 
get (@trans_y, selection, Y) , 

area_depth(Xl,yi,X2, Y2,Area_depth) ,  Y  >=  Yl,  Y  =<  Y2 ,  X  >=  XI,  X  =<  X2 , 
Area_depth  <  Depth. 


/*  Error  Reporting  Routines  for  Phase  Errors  and  Mission  Errors  */ 

/*  Modify  or  Delete  phase  selected  when  no  phases  have  been  specivied  */ 
invalid_option_report  (_)  destroy_error_objects,  fail. 
invalid_option_report (Error)  error_code (Error, Message) , 
new (@error_windowl, dialog ( 'Invalid  Option' ) ) , 
new  (OK,  button  ( 'OK'  ,message  ((iprolog,  destroy__error_objects) )  ) , 
new (Label, label) ,  send (Label, selection, Message) , 
send  ((ierror__windowl ,  append, Label ) , 
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send (OK, below, Label) ,  send (@error_windowl , open) . 


/*  There  is  an  error  in  the  phase  specification  */ 
invalid_phase_report {_)  : -  destroy_error_objects,  fail  * 
invalid_phase_report (Code)  error_c ode (Code, Message) , 

new (@error_window2, dialog ( 'Invalid  Phase' ) ) , 
new (OK, button! 'OK' /message (@prolog,destroy_error_objects) ) ) , 
new (Labell, label)  , 

send (Labell, selection, 'PHASE  ERROR;  THE  SPECIFIED  PHASE  IS  INVALID'), 
new (Labe 12 , label ) ,  send (Label2 , selection, Message) , 
send  ((aerror_window2,  append,  Labell)  ,  send  (Label2 ,  below,  Labell)  , 
send (OK , below, Label2 ) ,  send (@error_window2 , open) . 


/*  There  is  a  loop  in  the  mission  specification,  no  further  parsing  can  occur 
/*  until  the  loop  is  eliminated  */ 

invalid_mission_report (_,_)  destroy_error__objects,  fail . 
invalid_mission_report (Phase, Code)  error_code (Code, Message) , 

new (@error_window3, dialog! 'Mission  Error' ) ) , 
new (OK, button! 'OK' , message (@prolog,destroy_error_objects) ) ) , 
new (Labell , label ) , 

send(Labell, selection, 'MISSION  ERROR  IN  PHASE:  '), 

new (Label2, label) ,  send {Label2 , selection, Phase) , 

new (Labels , label ) ,  send (Labels , selection, Message) , 

send (@error_window3, append, Labell) ,  send (Label2, right, Labell) , 

send (Labels, below, Labell ) ,  send (OK, below, Labels) , 

send (Serror^windowS, open) . 


/*  There  is  at  least  one  error  associated  with  a  phase  of  the  mission 
/*  All  errors  associated  with  a  phase  are  displayed  at  once  */ 

phase_error_report (_, [LastError] )  object (LastError) , 

new (OK, but ton ( 'OK' , message (@prolog,destroy_error_objects) ) ) , 
send (OK, below, LastError) , 
send (@error_window4, open) . 

phase__error_report( Phase, Errors)  \+object (@error_window4) , 
new ( @error_window4 , dialog ( 'Mission  Errors ' ) ) , 

new(Labell, label) ,  send (Labell , selection, 'MISSION  ERRORS  IN  PHASE:  '), 
new (Labels, label) ,  send (Labels , selection, Phase) , 
send (@error_window4, append, Labell) , 
send (Labels , right, Labell ) , 

phase_error_report (Phase, [Labell  I  Errors] ) . 
phase__error_report (Phase,  [Previ ouster ror,  Current_error  i  Rest]) 
error_code  (Current__error, Message)  , 
new (Label , label ) ,  send (Label , selection, Message) , 
send (Label , below, Previous_error } , 
phase_error_report (Phase, [Label  I  Rest] ) . 


/*  Error  message  when  errors  are  detected  in  points  specified  */ 
/*  in  the  means  end  mission  generation  facility  */ 

invalid_point_report {_)  ; -  destroy_error_objects,  fail , 
invalid_point_report (Code)  error_code (Code, Message) , 
new (@error_window2, dialog! 'Invalid  Phase') ) , 
new (OK, button! 'OK' , message (@prolog, destroy _error  objects) ) ) , 
new (Labell, label) , 
send (Labell , selection, 

'POINT  ERROR:  ONE  OF  THE  SPECIFIED  POINTS  IS  INVALID'), 
new (Label2 , label ) ,  send (Label2 , selection, Message) , 
send (@error_window2, append, Labell) ,  send (Label2 , below, Labell ) , 
send (OK, below, Label2 ) ,  send ( @error_window2 , open) . 
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/*  Codes  to  match  error  names  to  an  output  message  */ 
error_code{no_phases, 'There  are  no  specified  phases  in  memory'). 
error_code  ( no__me__modi  fy  ^ 

'You  cannot  modify  a  mission  generated  by  the  Means  Ends  Help  Facility') . 
error_code(no_name, 'You  did  not  specify  a  name  for  the  phase'). 
error__code(duplicate_phase, 'A  phase  by  that  name  has  already  been  specified') 
error_code(no_successor, 'You  did  not  specify  one  of  the  successor  phases'). 
error_code(non__number, 'You  have  entered  a  non  number  in  a  numerical  field'). 
error_code(bad_heading, 'The  heading  must  be  between  0  and  360  degrees'). 
error_code(too_deep, 'The  depth  you  have  entered  is  too  deep  for  the  vehicle') 
error_code(bottom_hit, 'The  depth  you  specified  is  too  deep  for  this  area'). 
error__code(too_shallow, 'Depth  must  be  positive'). 

error_code (out_of_area, 'The  specified  point  is  outside  the  designated  operat¬ 
ing  area' ) . 

error_code (unreachable, 'Phase  not  reachable  from  start  phase'). 
error_code (loop, 'Phase  reachable  from  itself'). 
error_code( dangle, 'Successor  Phase  Undefined'). 

error_code (short__timer, 'Specified  time-out  inadequate  for  phase  completion'). 
error_code (no_complete, 'No  mission  completion  criteria  specified'). 
error_code(no_start, 'No  mission  starting  phase  specified'). 
error_code(no_file, 'You  did  not  specify  an  output  file'). 
error_code (hover_for_recovery , 

'Tube  recovery  must  be  preceeded  by  a  hover  phase'). 
error_code(long_recovery, 'Hover  preceeding  tube  recovery  too  distant'). 
error_code (short_recovery, 

'Hover  preceeding  tube  recovery  too  close  for  safety') . 
e  r ror_c  ode ( no  t_mi s  s i on_comp 1 e  t  e , 

'Successful  tube  recovery  must  be  last  phase'). 
error_code (displaced_search, 

'Search  phase  not  preceded  by  hover  at  the  same  location') . 
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/*  Destroy  all  Explicitly  Named  Objects  in  the  Initial  Window  */ 
destroy_start_window_objects  f ree (@start_dialog) , 

free  (@start__dialog)  , 
free  (@f ile_name)  , 
free (@path_x) ,  free (@path_y) , 
free (@path_xl) ,  free (@path_yl) , 
free (©picture) ,  free (@bmp) . 

/*  Destroy  Menu  Containing  Phase  Graph  Summary  */ 
destroy_phase_graph_window  f ree (@phase_dialog) , 
free (©phases) , 

free {©c_succ) ,  free (@a_succ) . 
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destroy_phase_suininary_menu  f ree  {@phase_dialog)  . 

/*  Destroy  Menu  For  Entering  Type  of  Phase  */ 
destroy _^hase_type_entry_objects  f ree (©type_dialog) . 

/*  Destoy  Menus  For  Entering  Individual  Phases  */ 
destroy_jphase_entry_objects  f ree {@parameter_dialog) , 
free(©phase_name) , 
free (@new_depth) , 
free (©time_out ) , 
free (©heading) , 
free (©done) , 
free (©reset) , 
free (@new_x) , 
free (©new_y) , 
free (©pointdialog) / 
free (©medialog) , 
free (©delete_dialog) , 
free  (@inodify_dialog) , 
free  (©me^route) , 
abolish (current_phase/l) , 
abolish (abort_successor/l) , 
abolish (coniplete_successor/l) , 

object  (@mouse__click_path) ,  send (©inouse_click_path,  clear) , 
fail . 

destroy_phase_entry__objects . 

/*  Destroy  Unspecified  Phase  Entry  Dialog  Box  Objects  */ 
destroy _unspecified_phase_entry_objects  : - 
free (@okl) , 
free (©name_entry ) , 
free (©name) . 

/*  Destroy  Code  Selection  Objects  Destroys  the  Window  used  to  */ 
/*  Specify  Desired  Output  Language  of  Strategic  Level  Code  */ 
destroy_code_selection_objects  f ree (©code_dialog) . 


/*  Destroy  Initialization  Entry  Objects  Destroys  Objects  in  */ 

/*  The  Initialization  Script  Generation  Window  */ 

destroy _initialization_entry_objects  : - 
free (@initialize_dialog) , 

free (©posture_x) ,  free (©posture_y ) ,  free (©posture_z) , 

free (©posture_phi) ,  f ree (©posture_theta) ,  f ree (©posture_psi ) , 

free (@dtlx) ,  free(©dtly) ,  free(©dtlz) , 

free (©dt2x) ,  f ree(©dt2y) ,  free (©dt2z) , 

free  (©gyro__error)  . 


/*  Destroy  ME  Objects  destroys  all  objects  associated  with  */ 

/*  The  Means  Ends  Analysis  help  facility  */ 

destroy_me_objects 

de  s  t  r  oy_s  e  a  r  ch_p  o  i  n  t_and_tube_ob  j  e  c  t  s , 

de  s  t  r  oy_ine_s  o  1  n_ob  j  e  c  t  s , 

de  s  t  roy_ina  i  n_jn e_menu_ob  j  e c  t  s  . 

/*  Destroy  Main  ME  Menu  Objects  destroys  all  objects  */ 

/*  associated  with  the  top  level  means  ends  help  menu  */ 
destroy_main_me__menu_objects 
free (©medialog) , 

f ree(@startx) ,  f ree (©starty ) ,  free (©startz) , 
free(©endx),  free(©endy),  free(©endz). 

/*  Destroy  Search  Point  and  Tube  Objects  destroys  all  objects  */ 
/*  associated  with  the  window  in  which  search  points  are  entered  */ 
/*  or  the  window  in  which  recovery  tube  data  is  entered  */ 

destroy _search__point_and_tube_objects 
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free (@pointdialog) ,  free (@tubedialog) , 
free (@trans_x) ,  free (@trans_y ) , 

free (@new__x) ,  f ree {@new_y) ,  free (@new_depth) ,  free (©heading) . 


/*  Destroy  ME  Soln  Objects  destroys  all  objects  associated  with  */ 
/*  window  in  which  the  means  ends  solution  is  displayed  */ 

destroy_me__soln_obj  ects  :  - 
free (©solutiondialog) , 
free (©solutionlabel) . 

clear_search_points  abolish (searchpoint/1) , 
clear_tube_data  abolish (tube_data/4) . 

/*  quit  destroys  all  objects  that  are  currently  in  existence  */ 
quit  : -  des troy_s ta rt_window_ob j ect s , 
destroy_phase_graph_window, 
reset_to_main_menu . 


/*  Reset  to  Main  Menu  Destroys  Menus  All  Menus  Except  the  Initial  One  */ 
reset_to_main_menu  abolish (current_phase /I ) , 

abolish {abort_successor/l ) , 

abolish {complete_successor/l) , 

destroy_phase_entry_obj  ects, 

des  t  r  oy^ha  se__type_en  t  ry_ob  j  ects, 

destroy_unspecif ied _phase_entry_objects, 

des  troy_error_ob j  ects, 

des troy _code_selection_objects, 

destroy_initialization_entry_objects, 

de  s  t  r  oy  _ine_ob  j  e  c  t  s . 

reset_to_inain_menu . 


/*  Destroy  error  dialog  box  */ 

des troy_error_obj ects  f ree (@error_windowl) , 

free(©error_window2) , 
free (© err or_window3) , 
free{@error_window4) , 
free (@ok) . 


/*  Reset  For  ME  Solution  destroys  the  means  ends  solution  */ 

/*  menu  and  all  of  the  objects  within  it,  and  abolishes  */ 

/*  all  facts  associated  with  a  means  ends  solution  so  that  */ 

/*  the  next  means  ends  solution  can  be  computed  and  displayed  */ 
reset__f or_me_solution  destroy_me_soln_objects,  erase__paths, 

abolish(phase/5) ,  abolish (start_phase/l) ,  asserta (start_phase ( 1 ) ) , 
abolish (next_phase/l ) ,  asserta (next_phase{l) ) , 


/*  Erase  Paths  clears  the  means  end  solution  path  and  the  */ 

/*  mouse  click  path  from  the  map  picture  */ 

erase_paths  \+object  (©mouse_click__path) , 

new(@mouse_click_path,path) ,  send(©mouse_click_path,pen,  2 )  , 
send (©picture, display, @mouse_click_path) ,  fail. 
erase_paths  : - 

free  (©me_route)  , 

send (©mouse_click_path, clear) . 
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successor ( Phasel , Phase2 ) 
successor ( Phasel , Phase2 ) 


phase  (Phasel, Phase2 . 
phase  ( Phasel ,  Phase2 )  . 


reachable { Phasel , Phase2 ) 
reachable ( Phasel , Phase2 ) 
able {Phase3 ,Phase2) . 


successor (Phasel, Phase2) . 
successor (Phasel , Phase3 ) ,  reach- 


transit_distance (Phase, Distance)  phase (Phase, Typel , [XI ,  Y1  i  Ll],_,_), 
\+same{Typel, 'GPS  Fix'),  \+same (Typel , 'Depth  Change'), 
position_predecessor (Phase, Predecessor) , 
phase ( Predecessor, [X2,  Y2  I  L2],_,_), 
distance (XI, Y1,X2,Y2, Distance) . 

transit_distance(Phase, Distance)  :-  phase ( Phase, Typel , [XI ,  Y1  I  L],_,_), 
\+same (Typel, 'GPS  Fix'),  \+same (Typel , 'Depth  Change'), 
startjhase  (Start)  ,  start_position  (X2,  Y2  ,D)  ,  same  (Start ,  Phase)  , 
distance(Xl,Yl,X2,Y2, Distance) . 

max_transit_distance ( Phase, Distance)  : - 

bagof (Dist, transit_distance (Phase, Dist) ,DList) , 
sort (DList,SortedList) , last (SortedList, Distance) . 

position_predecessor (Phase, Predecessor)  ; - 

(phase (Predecessor, Type, Phase, _) ;  phase (Predecessor, Type, Phase) ) , 
\+same (Type, 'GPS  Fix' ) ,  \+same (Type, 'Depth  Change' ) . 

position^predecessor (Phase, Predecessor)  : - 

(phase  (Phase2, Type, Phase, _) ;  phase (Phase2, Type, Phase) ) , 

(same (Type, 'GPS  Fix');  same (Type, 'Depth  Change')), 
posi tion_predecessor (Phase2 , Predecessor) . 

distance (XI , Y1 , X2 , Y2 , Distance)  : - 

X__plus_y__Squared  is  (X2-X1 )  *  (X2-X1 )  +  (Y2-Y1 )  *  (Y2-yi )  , 
sqrt (X_plus_Y_Squared, Distance) . 

approach_point (Xtube, Ytube, Theta, Xapp roach, Yapproach)  : - 
c3eg_to_rad  (Theta, The ta_r ad) , 

cos  (Theta_rad,  Cos_theta_rad) ,  sin  (Theta_rad,  Sin_theta_rad) , 
XapproachFloat  is  Xtube  -  15.0  *  Cos_theta_rad, 
round (Xapp roachFloat , Xapp roach) , 

YapproachFloat  is  Ytube  -  15.0  *  Sin_theta_rad, 
round (YapproachFloat , Yapproach) . 

perpendicular_point (Xtube , Ytube , Theta , Xperp , Yperp )  : - 
deg_to_rad (Theta, The ta_rad)  , 

cos (Theta_rad, Cos__theta_rad) ,  sin (Theta_rad, Sin_theta_rad) , 

XperpFloat  is  Xtube  -  15.0  *  Sin_theta__rad, 
round (XperpFloat , Xperp ) , 
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YperpFloat  is  Ytube  +  15.0  *  Cos_theta_rad, 
round (YperpFloat/Yperp) . 

deg_to_rad(Deg,Rad)  Rad  is  Deg  *  3.1415927  /  180.0. 

same(X,X) . 

string_to_num (String, Num)  name (String, Asc) , name (Nuin,Asc) , number (Num) . 

last{[X],X)  !. 

last([X  I  L],Y)  last(L,Y) . 

concatenate (SI, S2,S)  name (SI , ASl ) ,  name (S2 , AS2 ) , 
append {AS1,AS2, AS) ,name(S,AS) . 

list_concat {[],''). 

list_concat ( [XIL] ,S)  list_concat (L, Part) ,  concatenate (X, Part,S) . 

pad_to_30 (String, String)  name (String, List ) ,  length (List , Length) , 
Length  >=30,  !. 

pad__to_30 (String, New_string)  concatenate (String, '  \  Sub_string) , 
pad_to„30 (Sub_string,New_string) . 

member (X, [XIL] ) . 

member (X, [Y I L] )  member (X, L) . 

same_members ([],[]). 

same_members( [XI  I  L1],L2)  member (XI ,L2) ,  delete (XI, L2 ,NewL2 ) , 
same_members  (LI ,  NewL2 )  . 

subset ( [ ] ,L) . 

subset ( [XIL] ,L2)  singlemember (X, L2 ) ,  subset (L, L2 ) . 

singlemember (X, [XIL] )  1. 

singlemember (X, [Y I L] )  singlemember (X, L) . 


union ( [] ,L,L) . 

union( [XILl] ,L2,L3)  singlemember (X, L2 ) ,  !,  union (LI , L2 , L3 ) . 
union( [XILl] ,L2, [XIL3] )  union (LI , L2 , L3 ) . 

delete! terns ( [ ] ,L,L) . 

deleteitems( [XIL] ,L2,L3)  delete (X, L2 , L4 ) ,  delete! terns (L, L4 , L3 ) . 
delete (X, [],[]). 

delete(X, [XIL] ,M)  !,  delete (X, L,M) . 

delete(X, [YIL] , [YIM] )  delete (X, L,M) . 

construct_menu (Menu, Items, Callback)  member (Menul tern, Items) , 

send (Menu, append, menu_item(MenuItem,message{@prolog, Callback, Menultem) ) ) , 

fall. 

cons truct__menu  (_,_,_)  . 

/*  Automatic  Start  of  Program  Upon  File  Load  */ 


go. 

/*  End  of  File  mission_expert.pl  */ 
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/* 


Program: 

Authors : 

Revised: 

System: 

Compiler: 

Compilation: 

Invocation: 

[  input  file: 
[output  file: 


AUV  strategic  level  program 
Brad  Leonhardt  Duane  Davis 
1  August  96 

SUN  Voyager  Solaris  2.4  OS;  SGI  Irix  5.3 
Sun  C;  IRIX  cc 

cc  missionjl.c  -o  mission_pl 

mission_pl  [normally  invoked  automatically  by  expert  system] 
command^st rings ] 

name  is  specified  by  user  and  contained  in  commandos t rings] 


This  code  is  used  to  create  Prolog  code  to  run  the  Phoenix  Autonomous  Vehicle 
It  can  be  run  with  the  prolog  mission__expert  system  which  creates  a  data 
file  for  use  in  the  mission  generator. 


tinclude  <string.h> 
tinclude  <stdio.h> 
^include  <ctype.h> 


#define  DATA_FILE  "command_st rings"  /*  Expert  System  Output  */ 

y^***************************************-*********************************it**  j 

/*  function  prototypes  */ 


int 

main 

0; 

void 

depth__change 

0; 

void 

hoverpoint 

0; 

void 

waypoint 

0; 

void 

sonar_search 

{); 

void 

get  gps  fix 

{); 

void 

rotate_sonar__search 

0; 

void 

course 

0; 

void 

wait 

0; 

void 

tube_recovery 

0; 

void 

parse_command 

0; 

void 

time_out 

{); 

void 

next_phase 

0; 

void 

f ail_phase 

0; 

FILE  * 

filejtr; 

FILE  * 

out_file _ptr; 

FILE  *  standalone_out_f ile_jptr; 


char  command 

char  out_file_name 

char  standalone_out_f ile_name 

char  shell_cmd 

char  phase_name 

char  nextphase 

char  failphase 

char  cmd 


[200] ; 
[200] ; 
[200]  ; 
[200] ; 
[200] ; 
[200] ; 
[200] ; 
[200] ; 


int  variablel , variable2 , variables , variable4 , variables ; 
int  i ; 
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/★*********************************** ***************************************^ 
/*  Parse  command  breaks  input  string  into  variables  to  be  used  in  program 
*/ 

/************★**********★**★*****************★*****************★********★***/ 

void  parse_command  (cmd) 
char  *  cmd; 

C 

sscanf(cmd,  "%s  %s  %s  %s  %d  %d  %d  %d  %d" , 

command, phase_name , nextphase , f ai Iphase , 

Scvariablel ,  &variable2 ,  ^variables ,  &variable4 ,  ^variables )  ; 

/*  printf  {"COMMAND  %s  PHASE  NAME  %s  VARl  %d  VAR2  %d  VAR3  %d  VAR4  %d  VAR5  %d", 
command, phase_name, nextphase, failphase, 
variablel , variable2 , variables , variable4 , variables ) ; 

V 

} 

/**************************★************************************************ ^ 


int  main  () 

{ 

char  cmd  [200];  /*  input  line  string  of  characters  */ 

/*  Open  file  generated  from  expert  system  */ 

if  ((file_ptr  =  fopen  (DATA_FILE,  "r"))  ==  ({FILE  *)  0)) 

{ 

printf  {"input_read_path:  file  open  failure! \n") ; 
return; 

) 

fgets  {cmd,200,file_ptr)  ;  /*  Read  a  line  of  data  */ 

parse^command  (cmd) ; 

/*Create  an  output  file  for  mission  and  copy  mission  controller  data  */ 

if  (strcmp  (command, "file_name" )  ==  0) 

{ 

strcpy  (out_f  ile__name,phase_name)  ; 

strcpy  (standalone_out_file_name,  "standalone_" )  ; 

strcat  {standalone_out_f  i.le_name,  phase_name)  ; 

strcat  (shell__cmd,  "cp  control ler__pl . script  ")  ; 

strcat  (shell_cmd,  out_f  ile_name)  ; 

printf ("Shell  command  : %s\n" , shell_cmd) ; 

system (shell_cmd) ; 

} 

/*  Create  output  file  for  standalone  strategic  level  code  */ 

strcpy  {shell_cmd,  "cp  con troller_standalone_pl  .script  "); 

strcat  (shell_cmd,  "standalone^.")  ; 

strcat  (shell_cmd,  out_f ile_name)  ; 

printf ("Shell  command  :%s\n",shell_cmd); 

system  (shell_cmd)  ; 

printf ("Output  files  created\n"); 

if  ( ( (out_f ile_ptr  =  fopen  (out_f ile_name,  "a"))  ==  {{FILE  *}  0))  I! 

{ (standalone_out_file_ptr  =  fopen  (standalone_out_f ile_name,  "a"))  == 
((FILE  *)  0))) 

{ 
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printf {"INVALID  INPUT  FILE  NAME  %s\n" , out_f ile^name) ; 
fclose  (file_ptr); 
return (0) ; 


fprintf  (out_file_ptr, 

"\t\t\t  asserta (current_j}hase (%s) ) , \n" ,nextphase) ; 
fprintf  (out_file_ptr,  "\t\t\t  asserta (succeed(O) ), \n" ) ; 
fprintf  (out_file_ptr,  "\t\t\t  asserta (abort (0) ). \n\n") ; 

fprintf  (standalone_out_file_ptr, 

"\t\t\t  asserta  (current_:phase  (%s) )  ,  \n"  ,nextphase) ; 
fprintf  (standalone_out_file_ptr,  "\t\t\t  asserta (succeed (0) ) An") ; 
fprintf  {standalone_out_file_ptr,  "\t\t\t  asserta (abort (0) ). \n\n" ) ; 

while  ((fgets  (cmd,  200,  file^tr)  !=  NULL)  )  /*  read  next  line  */ 

{ 

printf ("Input  file  string  %s\n",cmd); 
parse_command(cind)  ; 

fprintf  {out_file_j)tr,  "%%  %s\n\n\n",phase_name) ; 

fprintf  (out_file_ptr,  "execute__phase  (%s)  \n"  ,phase_naine) ; 

fprintf  (out_file_ptr,  "\t\t  nl,printsc ( 'PHASE") ; 

fprintf  (out_file_ptr,  "  %s  STARTED. '), \n" ,phase_name) ; 

fprintf  (standalone_out_file_j)tr,  "%%  %s\n\n\n" , phase_name) ; 

fprintf  (standalone_out_file_ptr,  "execute_phase (%s) \n" , phase^name) ; 

fprintf  (standalone_out_file_ptr,  "\t\t  nl ,printsc (' PHASE" ) ; 

fprintf  (standalone_out_file_ptr,  "  %s  STARTED. ') An" , phase_name) ; 

/*  Determine  which  template  to  use  * / 

if  (strcmp  (command, "dept h_change" )  ==  0) 
depth_change  ( ) ; 

else  if  (strcmp  (command, "hoverpoint")  ==  0) 
hoverpoint  ( ) ; 

else  if  (strcmp  (command, "waypoint")  ==  0) 
waypoint  ( ) ; 

else  if  (strcmp  (command, "sonar_search" )  ==  0) 
sonar_search  ( ) ; 

else  if  (strcmp  (command, "get  gps  fix")  ==  0) 
get_gps_fix  (); 

else  if  (strcmp  (command, "rota te_sonar_search" )  ==  0) 
rotate__sonar_search  (); 

else  if  (strcmp  (command, "course")  ==  0) 
course  ( ) ; 

else  if  (strcmp  (command, "wait" )  ==  0) 
wait  0; 

else  if  (strcmp  (command,  "tube_recovery" )  ==  0) 
tube_recovery  ( ) ; 


else 

{ 

printf ("INVALID  PHASE  INPUT  %s\n" , command) ; 

fprintf  (file_ptr,  "\n"); 

fclose  (file__ptr); 

fprintf  (out_f ile_ptr,  "\n"); 

fclose  {out_f ile_ptr)  ; 

fprintf  (standalone_out_file_ptr,  "\n") ; 

fclose  {standalone_out_file_ptr) ; 

return  (0 )  ,* 
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} 


} 

fprintf  (file_ptr,  "\n"); 

fclose  (filG_ptr); 

fprintf  {out_filG_ptr,  "\n"); 

fclose  (out_f ile_ptr) ; 

fprintf  (standalone_out_file_ptr,  "\n") ; 

fclose  {standalone_out_file_ptr) ; 


/ 

*  * 

*/ 

/* 

V 

/ 

*  * 
*/ 


Functions  that  produce  the  Prolog  code  for  the  various  types  of  phases 


void  depth_change  ( ) 

{ 


fprintf  (out_file_ptr,  "\t\t\tood ( 'depth  "); 
fprintf  (out_file_ptr,  "%d' , Reply) , \n",variable2) ; 

fprintf  (out_file_ptr,  "\t\t\t  printsc  ( 'depth  %d' ),  \n'',variable2)  ; 
fprintf  (out_file_ptr,  "\t\t\tood( 'start_timer  ") ; 
fprintf  (out_file_ptr,  ''%d',Reply),\n",variablGl); 
fprintf  (out_file_ptr,  "\t\t\t  repeat,"); 

fprintf  (out_filG_ptr,  "phase_completed(%s) . \n\n" ,phase_narae) ; 

fprintf  (out_file_ptr,  "phase_complGted(%s) \n",phasG_name) ; 
fprintf  (out_file_ptr, 

"\t\t  ood('ask_depth_reached' , Reply ) ,RGply==l,\n") ; 

fprintf  (out_file_ptr,  "\t\t\t  printsc ( 'DEPTH  REACHED. '), \n" ) ; 
fprintf  (out_file_ptr,  "\t\t\t  asserta (succeed (") ; 
fprintf  (out_file_ptr,  '%s) ) . \n\n" , phase_naine) ; 


fprintf  (standalone_out_file_ptr,  "\t\t\tood( 'depth  "); 
fprintf  (standalone_out_file_ptr,  "%d' , Reply) , \n" , variable! ) ; 
fprintf  (standalone_out_file_ptr, 

"\t\t\t  printsc ( 'depth  %d' ), \n", variable!) ; 
fprintf  (standalone_out_file_ptr,  "\t\t\tood( 'start_timer  " ) ; 
fprintf  (standalone_out_file_ptr,  "%d' , Reply) \n" ,variablel) ; 
fprintf  (standalone_out_file_ptr,  "\t\t\t  repeat,"); 

fprintf  (standalone_out_file_ptr,  "phase_coinpleted(%s) . \n\n",phase_name) ; 

fprintf  (standalonG_out_f ile_ptr,  "phase_coinpleted(%s)  \n",phase_naine) ; 
fprintf  (standalone_out_file_ptr, 

"\t\t  ood('ask_depth_reached' , Reply ) ,Reply==l, \n") ; 

fprintf  (standalone_out_file_ptr, 

"\t\t\t  printscC 'DEPTH  REACHED. '), \n") ; 
fprintf  (standalone_out_file_ptr,  "\t\t\t  asserta (succeed (") ; 

fprintf  (standalone_out_file_ptr,  "%s) ) . \n\n" ,phase_namG) ; 

tiine_out  (); 

next_phase  ( ) ; 

fail_phase  (); 


) 
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void  hoverpoint  ( ) 


fprintf  (out_file^tr,  "\t\t\tood ( 'hover  "); 
fprintf  (out:_file_ptr,  "%d  %d  %d  %d' ,  Reply)  ,  \n"  , 
variable2 , variables , variable4 , variables ) ; 
fprintf  (out_file_ptr.  "\t\t\t  printsc  ( 'hover  %d  %d  %d  %d')An", 
variables , variables , variable4 , variables ) ; 
fprintf  {out_file_ptr,  "\t\t\tood ( ' start^timer  ") ; 
fprintf  (out_filejptr,  ''%d' , Reply) ,  Xn" ,  variablel) ; 
fprintf  (out_file_ptr,  "\t\t\t  repeat,"); 

fprintf  {out_file_ptr,  "phase^completed (%s) . \n\n" ,phase_name)  ; 


fprintf  {out_file_ptr,  "phase_completed (%s) \n" , phase_name) ; 
fprintf  (out_file_ptr, 

"\t\t  ood('ask__hoverpt_reached' , Reply) , Reply==l , \n" ) ; 

fprintf  (out_file_ptr,  "\t\t\t  printsc ( 'HOVERPOINT  REACHED. '), \n" ) ; 
fprintf  (out_file_p)tr,  "\t\t\t  asserta (succeed( ") ; 
fprintf  {out_file_ptr,  "%s))  .  \n\n" ,phase_naine)  ; 


fprintf  (standalone_out_f ile_ptr,  "\t\t\tood ( 'hover  "); 
fprintf  (standalone_out_f ile_ptr,  "%d  %d  %d  %d' , Reply ), \n" , 
variables , variables , variable4 , variables ) ; 
fprintf  (standalone__out_f ile_ptr, 

"\t\t\t  printsc ( 'hover  %d  %d  %d  %d'),\n", 
variables , variables ,variable4, variables) ; 
fprintf  (standalone_out_file_ptr,  "\t\t\tood ( ' start^timer  "); 
fprintf  {standalone_out_f ilejitr,  "%d', Reply) , \n" , variablel ) ; 
fprintf  (standalone_out_file_ptr,  "\t\t\t  repeat,") ; 

fprintf  (standalone_out_file_ptr,  "phase_completed(%s) . \n\n" , phase^name) 


fprintf  (standalone_out_f  ile__ptr,  "phase_completed(%s)  \n"  ,phase_naine)  ; 
fprintf  {standalone_out_f ile_ptr, 

"\t\t  ood('ask_hoverpt_reached' ,  Reply)  ,Reply==l ,  \n" )  ; 

fprintf  (standalone_out_f ile_ptr, 

"\t\t\t  printsc { 'HOVERPOINT  REACHED. '), \n" ) ; 
fprintf  {standalone_out_file_ptr,  "\t\t\t  asserta (succeed (" ) ; 
fprintf  (standalone_out_file_ptr,  "%s)) . \n\n",phase_naine) ; 

time_out  (); 

next_phase  (); 

fail_phase  (); 


void  waypoint  () 

{ 

fprintf  (out_file_ptr,  "\t\t\tood ( 'waypoint  "); 
fprintf  {out_file_jptr,  "%d  %d  %d' , Reply ),  \n" , 
variables , variables , variable4 ) ; 

fprintf  (out_file_ptr,  "\t\t\t  printsc ( 'waypoint  %d  %d  %d'),\n", 
variables , variables , variable4 ) ; 
fprintf  (out_file_ptr,  "\t\t\tood ( 'start^timer  "); 
fprintf  (out_file_ptr,  "%d' , Reply ), \n",variablel ) ; 
fprintf  (out_file_ptr,  "\t\t\t  repeat,"); 

fprintf  {out_file_j)tr,  "phase_coinpleted(%s)  . \n\n" ,phase_naine)  ; 
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fprintf  {out_file_ptr,  "phase_completed(%s)  \n",phase_name)  ; 
f print f  (out_file_ptr, 

"\t\t  ood  ( '  ask_waypt_reached' , Reply)  ,  Reply==:l ,  \n"  )  ; 

fprintf  (out_file_ptr,  "\t\t\t  printsc ( 'WAYPOINT  REACHED. '), \n" ) ; 
fprintf  (out_file_ptr,  "\t\t\t  asserta (succeed! ") ; 
fprintf  (out_file_ptr,  "%s) ) . \n\n" ,phase_name) ; 


fprintf  (standalone_out__file_ptr,  "\t\t\tood( 'waypoint  "); 
fprintf  {standalone_out_f ile_ptr,  "%d  %d  %d Reply ), \n" , 
variable2 , variables , variable4 ) ; 
fprintf  (standalone_out_f ile__ptr, 

"\t\t\t  printsc ( 'waypoint  %d  %d  %d'),\n", 
variables /Variables ,variable4) ; 
fprintf  (standalone_out_file_ptr/  "\t\t\tood( 'start_tiraer  "); 
fprintf  (standalone_out__file_ptr,  "%d' /Reply) / \n" / variablel) ; 
fprintf  (standalone_out_f ile_jDtr,  "\t\t\t  repeat,") ; 
fprintf  (standalone_out_file_ptr/  "phase_completed(%s)  . \n\n",phase_naine) 


fprintf  (standalone_out_file_ptr/  "phase_completed{%s) \n" ,phase_name) ; 
fprintf  (standalone_out__f ile_ptr, 

"\t\t  ood( 'ask_waypt_reached' , Reply)  /Reply==l, \n" )  ; 

fprintf  {standalone_out_f ile_ptr/ 

"\t\t\t  printsc ( 'WAYPOINT  REACHED. ')/ \n" ) ; 
fprintf  (standalone_out_file_ptr/  "\t\t\t  asserta (succeed (" ) ; 
fprintf  (standalone__out_file_ptr/  "%s) )  .  \n\n"  ,phase_naine)  ; 

time_out  (); 

next^phase  (); 

fail_phase  (); 


void  sonar_search  () 

{ 

fprintf  {out_file_ptr/  "\t\ t\tood ( ' sonar_search ' /Reply) , \n") ; 
fprintf  (out_file_ptr/  "\t\t\t  printsc (' sonar_search  l')/\n"); 

fprintf  (out_file_ptr,  "\t\ tXtood ( ' start_timer  "); 
fprintf  (out^file^ptr,  "%d' /Reply) , \n" / variablel ) ; 
fprintf  (out_file_ptr/  "\t\t\t  repeat,"); 

fprintf  (out_filejtr,  "phase_coitipleted (%s)  . \n\n" ,phase_name)  ; 


fprintf  (out_f ile^ptr,  "phase_completed{%s)  \n",phase_name)  ; 
fprintf  (out_file__ptr, 

"\t\t  ood( 'ask_sonar_search_complete' ,Reply) ,  ")  ; 

fprintf  (out_file_ptr,  "Reply==l, \n") ; 

fprintf  (out_file_ptr,  "\t\t\t  printsc ( 'SONAR  SEARCH  COMPLETE. ' ) , \n") ; 
fprintf  (out_file_ptr,  "\t\t\t  asserta (succeed!") ; 
fprintf  (out_file_j)tr,  "%s)  )  .  \n\n" ,phase_naine)  ; 


fprintf  (standalone_out__file_ptr,  "\t\t\tood( 'sona research' , Reply) , \n") ; 
fprintf  (standalone_out_f ile_ptr, 

"\t\t\t  printsc ( 'sonar_search  ! ' ) , \n") ; 
fprintf  (standalone_out_filejtr/  "\t\t\tood( 'start_tiiner  "); 
fprintf  (standalone_out_file_ptr,  "%d', Reply) , \n" , variablel ) ; 
fprintf  (standalone_out_file„ptr,  "\t\t\t  repeat, ") ; 
fprintf  {standalone_out_f  ile__ptr/  "phase_conipleted(%s)  .  \n\n" ,  phase^name) 
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fprintf  (standalone_out_f ile_ptr,  "phase_coinpleted{%s) \n" /Phase^name) ; 
fprintf  {standalone_out_f ile_pt:r, 

"\t\t  ood{ 'ask_sonar__search_complete' /Reply)  ; 

fprintf  (standalone_out_file_ptr,  "Reply==l/ \n" ) ; 
fprintf  {standalone_out_f ile_ptr, 

"\t\t\t  printsc( 'SONAR  SEARCH  COMPLETE \n") ; 
fprintf  (standalone_out_file_ptr,  "\t\t\t  asserta (succeed!") ; 
fprintf  (standalone_out_file_ptr,  "%s) } . \n\n" ,phase_naine) ; 

time_out  ()  ; 

next_phase  ()  ; 

fail_phase  (); 


void  get_gps.,.,f ix  () 

{ 

fprintf  (out_file_ptr/  "\t\t\tood ( 'gefclgps_fix' , Reply) , \n") ; 
fprintf  (out_file_ptr,  "\t\t\t  printsc ( 'get_gps_f ix  !')/\n"); 

fprintf  (out__file jtr,  "\t\t\ tood { ' start_tiiner  ")  ; 
fprintf  {out__f ile_ptr,  "%d'  /  Reply)  /  \n" ,  variablel)  ; 
fprintf  {out_filejtr,  "\t\t\t  repeat/"); 

fprintf  (out_f ile_ptr/  "phase^completed {%s) , \n\n" /phase_name) ; 


fprintf  (out_file_ptr/  "phasG_coinpleted (%s)  \n" / phase_name)  ; 
fprintf  (out_file_jptr/  "\t\t  ood(  'ask  get._gps_  fix'  .Reply) .  ; 

fprintf  {out_f  ile^tr/  "RGply==l/ \n" )  ; 

fprintf  (out_file_ptr/  "\t\t\t  printsc('G  P  S  FIX  OBTAINED. '), \n" ) ; 
fprintf  (out_f ile_ptr/  "\t\t\t  asserta (succeed!") ; 
fprintf  (out_file_ptr/  "%s)  )  .  \n\n" /phase_naine)  ; 


fprintf  (standalone_out_f ile_ptr,  "\t\t\tood( 'get_^ps_f ix' /Reply) ,\n") ; 
fprintf  (standalone_out_file_ptr/  "\t\t\t  printscf'get  crps  fix  i')/\n") 
fprintf  (standalone_out_file_ptr/  "\t\t\tood( 'start_timer  "); 
fprintf  (standalone_out_f ile_ptr/  "%d' /Reply) / \n" , variablel) ; 
fprintf  (standalone_out__f ile__ptr,  "\t\t\t  repeat/")  ; 

fprintf  (standalone_out_f  ile_j)tr/  "phase_coiTipleted(%s)  .  \n\n"  /  phase^name)  ; 


fprintf  (standalone_out_f ile^ptr,  "phase_completed(%s) \n" ,phase__name) ; 
fprintf  (standalone_out__f ile^ptr, 

"\t\t  ood( 'ask_get_gps_fix' /Reply) /") ; 

fprintf  (standalone_out_f ile_ptr/  "Reply==l / \n" ) ; 
fprintf  (standalone_out_f ilejtr/ 

"\t\t\t  printsc{'G  P  S  FIX  OBTAINED. '), \n" ) ; 
fprintf  (standalone_out_f ile^ptr,  "\t\t\t  asserta (succeed! ") ; 
fprintf  (standalone_out_f ile_ptr/  "%s) ) . \n\n" /phase__name) ; 

tiine_out  (); 

next__phase  ( )  ; 

fail_phase  ()  ; 


} 
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void  rotate_sonar_search  ( ) 

{ 

fprintf  (out_file_ptr,  "XtXtXtood (' rotate_search' , Reply ), \n" ) ; 
fprintf  (out_file_ptr,  "XtXtXt  printsc  ( '  rotace_search  !'),Xn''); 
fprintf  (out_file_ptr,  "XtXtXtood ( 'start_tiiner  "); 
fprintf  (out_file_ptr,  "%d' , Reply ),  Xn"  ,  variablel )  ,- 
,  fprintf  (out_file_ptr,  "XtXtXt  repeat,"); 
fprintf  (out_file_ptr,  "phase_completed (%s) . XnXn" ,phase_naine) ; 


fprintf  (out_file_ptr,  "phase_completed(%s) Xn",phase_name) ; 
fprintf  (out_file_ptr,  "XtXt  ood("'); 

fprintf  (out_file_ptr,  "ask_rotate_search_coinplete' , Reply ) ,Reply==l, Xn" ) 
fprintf  (out_filej>tr,  "XtXtXt  printsc ( 'ROTATE  SEARCH  COMPLETE. '), Xn" ) 
fprintf  {out_file_ptr,  "Xt\t\t  asserta (succeed! ") ; 

fprintf  (out_file_ptr,  *%s) ) . XnXn",phase_name) ; 


fprintf  (standalone_out_file_ptr,  "XtXt\tood('rotate_search', Reply) ,\n") 
fprintf  (standalone_out_f ile_ptr, 

"XtXtXt  printsc ( 'rotate_search  !'),Xn"); 
fprintf  (standalone_out_file_ptr,  "\t\t\tood( 'start_timer  ") ; 
fprintf  {standalone_out_file_ptr,  "%d' , Reply) , Xn", variablel) ; 
fprintf  (standalone_out_file_ptr,  "XtXtXt  repeat, "); 
fprintf  (standalone_out_file_ptr,  "phase_coinpleted(%s) . Xn\n",phase_name) 


fprintf  (standalone_out_file_ptr,  "phase_coinpleted(%s) \n",phase_name) ; 
fprintf  (standalone_out_file_ptr,  "\tXt  ood('"); 

fprintf  (standalone_out_file_ptr, 

"as)c_rotate_search_complete' , Reply ) , Reply==l , Xn" ) ; 
fprintf  (standalone_out_f ile_ptr, 

"XtXtXt  printsc ( 'ROTATE  SEARCH  COMPLETE .'), Xn" ) ; 
fprintf  (standalone_out_file_ptr,  "XtXtXt  asserta  (succeed (")  ,- 
fprintf  (standalone_out_file_ptr,  "%s) ) . XnXn" ,phase_naine) ; 

time_out  (); 

next_pliase  (); 

fail_phase  (); 


void  course  ( ) 

{ 

fprintf  (out_file_ptr,  "Xt\tXtood( 'course  "); 
fprintf  (out_file_ptr,  "%d' , Reply) ,  \n",variable2) ; 

fprintf  (out_file_ptr,  "XtXtXt  printsc (' course  %d  ! ') ,Xn",variable2) ; 
fprintf  (out_file_ptr,  "XtXtXtood! 'start_tiiner  ") ; 
fprintf  (out_file_ptr,  "%d' , Reply) , Xn", variablel) ; 
fprintf  (out_file_ptr,  "XtXtXt  repeat,"); 

fprintf  (out_file_ptr,  "phase_coinpleted (%s) . XnXn" ,phase_name) ; 


fprintf  (out_file_ptr,  "phase_completed(%s)  Xn",pliase_naine)  ; 
fprintf  (out_file_ptr,  "XtXt  ood('"); 

fprintf  (out_file_ptr,  "as)c_course_reached' , Reply ) ,Reply==l, Xn") ; 
fprintf  (out_file_ptr,  "XtXtXt  printsc ( 'COURSE  CHANGE  COMPLETE. '), Xn" ) 
fprintf  (out_file_ptr,  "XtXtXt  asserta (succeed!") ; 
fprintf  (out_file_ptr,  "%s) ) . XnXn" , phase_name) ; 
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fprintf  (standalone_out_file_ptr,  "XtXtXtoodl 'course  "); 
fprintf  (standalone_out_file_ptr,  ''%d', Reply)  An", variable2)  ; 
fprintf  (standalone_out_file_ptr, 

"\t\t\t  printsc ( 'course  %d  ! ' ) , \n",variable2 ) ; 
fprintf  (standalone_out_file_ptr,  "\t\t\tood( 'start_timer  "); 
fprintf  (standalone_out_file_ptr,  "%d', Reply) ,\n",variablel) ; 
fprintf  (standalone_out_file_ptr,  "\t\t\t  repeat,"); 

fprintf  {standalone_out_file_ptr,  "phase_coinpleted{%s) . \n\n",phase_naine) ; 


fprintf  ( standalone_out_f ile_ptr,  "phase_coitipleted(%s)  \n",phase_naine) ; 
fprintf  (standalone_out_file_ptr,  "\t\t  ood('"); 

fprintf  {standalone_out_file_ptr, 

"aslc_course_reached' ,  Reply ) ,  Reply==l ,  \n" )  ; 
fprintf  (standalone_out_file_ptr, 

"\t\t\t  printsc ( 'COURSE  CHANGE  COMPLETE. '), \n" ) ; 
fprintf  (standalone_out_file_ptr,  "\t\t\t  asserta (succeed(") ; 
fprintf  (standalone_out_file_ptr,  "%s) ) . \n\n" ,phase_naine) ; 

t  iine_ou  t  ( )  ; 

next_phase  (); 

fail_phase  (); 


void  wait  () 

{ 

fprintf  (out_file_ptr,  "\t\t\tood( 'start_tiiner  ") ; 
fprintf  {out_file_ptr,  "%d' , Reply) , \n",variablel) ; 
fprintf  (out_file_ptr,  "\t\t\t  .  repeat,"); 

fprintf  (out_file_ptr,  "phase_coinpleted(%s) .\n\n",phase_name) ; 


fprintf  (out_file_ptr,  "phase_coinpleted(%s) \n",phase_name) ; 
fprintf  {out_file_ptr,  "\t\t  ood('"); 

fprintf  (out_file_ptr,  "aslc_tiine_out' , Reply )  ,Reply==l,  \n" )  ; 
fprintf  {out_file_ptr,  "\t\t\t  printsc ( 'WAIT  COMPLETE. '), \n") ; 
fprintf  (out_file_ptr,  "\t\t\t  asserta {succeed(") ; 
fprintf  {out_file^tr,  "%s) ) .  \n\n",phase_name)  ; 


fprintf  (standalone_out_file_ptr,  "\t\t\tood( 'start_timer  " ) ; 
fprintf  {standalone_out_file_ptr,  "%d' , Reply) , \n" ,variablel ) ; 
fprintf  (standalone_out_file_ptr,  "\t\t\t  repeat,"); 

fprintf  (standalone_out_file_ptr,  "phase_coinpleted(%s) .  \n\n",phase_naine)  ; 


fprintf  (standalone_out_file_ptr,  "phase_coinpleted{%s)  \n"  ,phase_naine)  ; 
fprintf  (standalone_out_f ile_ptr,  "\t\t  ood('"); 

fprintf  (standalone_out_file_ptr,  "as)c_time_out' , Reply ) ,Reply==l, \n") ; 
fprintf  (standalone_out_file_ptr, 

"\t\t\t  printsc( 'WAIT  COMPLETE .'), \n" ) ; 
fprintf  (standalone_out_file_ptr,  "\t\t\t  asserta (succeed (") ; 
fprintf  (standalone_out_file_ptr,  "%s) ) . \n\n" ,phase_name) ; 

next_phase  ( ) ; 
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void  tube_recovery  ( ) 

{ 

fprintf  (out_file_pt:r,  "\t\c\Cood ( ' tube_recover  %d  %d  %d  %d' , Reply) , \n" , 
variablG2,  variables,  variable4,  variables); 
fprintf  (out_file_ptr, 

"\t\t\t  printsc( 'tube_recover  %d  %d  %d  %d  !')/\n", 
variable2,  variables,  varlablG4,  variables); 
fprintf  (out_file_ptr,  "\t\t\tood('start_tiiner  "); 
fprintf  (out_file_ptr,  ''%d' , Reply) ,  \n",variablel+2S0)  ; 
fprintf  (out_file_ptr,  "\t\t\t  repeat,"); 

fprintf  (out_file_ptr,  "phase_completed (%s) . \n\n" ,phase_naine) ; 


fprintf  (out_file_ptr,  "phase_completed(%s) \n",phase_name) ; 
fprintf  (out_file_ptr,  "\t\t  ood('"); 

fprintf  (out_file_ptr,  "ask_recovery_coinplete' , Reply) ,Reply==l, \n") ; 
fprintf  (out_filej)tr,  "\t\t\t  printsc ( 'TUBE  RECOVERY  COMPLETE. '), \n") 
fprintf  (out_file_ptr,  "\t\t\t  asserta (succeed(") ; 
fprintf  (out_file_ptr,  "%s) ) . \n\n",phase_name) ; 


fprintf  (standalone_out_file_ptr, 

"\t\t\tood( 'tube_recover  %d  %d  %d  %d ', Reply ) ,\n", 
variable2,  variables,  variable4,  variables); 
fprintf  (standalone_out_file_ptr, 

"\t\t\t  printsc ( 'tube_recover  %d  %d  %d  %d  !'),\n", 
variable2,  variables,  variable4,  variables); 
fprintf  (standalone_out_file_ptr,  "\t\t\tood( 'start_tiiner  "); 
fprintf  (standalone_out_file_ptr,  "%d' , Reply) , \n" , variablel+2S0 ) ; 
fprintf  (standalone_out_f ile_ptr,  "\t\t\t  repeat,"); 

fprintf  (standalone_out_file_ptr,  "phase_coinpleted(%s) .\n\n",phase_naine) 


fprintf  {standalone_out_file_ptr,  "phase_completed(%s) \n" ,phase_name) ; 
fprintf  (standalone_out_f ile_ptr,  "\t\t  ood('"); 

fprintf  (standalone_out_file_ptr, 

"ask_recovery_complete' , Reply) ,Reply==l, \n" ) ; 
fprintf  (standalone_out_f ile_ptr, 

"\t\t\t  printsc ( 'TUBE  RECOVERY  COMPLETE .'), \n" ) ; 
fprintf  {standalone_out_file_ptr,  "\t\t\t  asserta (succeed (") ; 
fprintf  (standalone_out_file_ptr,  "%s) ) . \n\n" ,phase_naine) ; 

time_out  () ; 

next  jihase  { )  ,- 

fail_phase  (); 


void  time_out  () 

{ 

fprintf  (out_file_ptr,  "phase_coinpleted(%s) \n",phase_name) ; 
fprintf  (out_file_ptr, 

"\t\t  ood ('ask_time_out', Reply ), Reply ==1, \n") ; 

fprintf  (out_file_ptr,  "\t\t\t  printsc ( 'PHASE  %s  ",phase_name) ; 
fprintf  (out_file_ptr,  "ABORTED  DUE  TO  TIME  Otfr.'),\n"); 
fprintf  (out_file_ptr,  "\t\t\t  asserta (abort (%s)).\n\n",phase_name); 


fprintf  (standalone_out_file_ptr,  "phase_conipleted(%s) \n",phase_name) ; 
fprintf  (standalone_out_f ile_ptr, 

"\t\t  ood ( 'ask_tiine_out Reply) , Reply==l , \n") ; 

fprintf  (standalone_out_f ile_ptr, 

"\t\t\t  printsc ('PHASE  %s  " , phase_name) ; 
fprintf  (standalone_out_file_ptr,  "ABORTED  DUE  TO  TIME  OUT.  ' )  ,  \n"  )  ,- 
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fprintf  (standalone_out_file_ptr,  "XtXtXt 
asserta  (abort  (%s)  )  .  XnXn'',phase_name)  ; 

} 


void  next_phase  ( ) 
( 


fprintf  {out_file_ptr,  "next_phase (%s) Xn" ,phase_name) ; 

fprintf  (out_file_ptr,  "XtXt  succeed(%s) , Xn",phase_name) ; 

fprintf  (out_file_ptr,  "XtXtXt  retract (current_phase(") ; 

fprintf  (out_file_ptr,  "%s) ) , \n",phase_name) ; 

fprintf  (out_file_ptr,  "XtXtXt  asserta {current_phase(") ; 

fprintf  {out_file_ptr,  "%s) ) . XnXn",nextphase) ; 


fprintf  (standalone_out_file_ptr,  "next_phase (%s) Xn" ,phase_naine) ; 
fprintf  (standalone_out_file_ptr, 

"\tXt  succeed(%s) , Xn",phase_name) ; 

fprintf  (standalone_out_file_ptr,  "XtXtXt  retract (current_phase (") ; 

fprintf  (standalone_out_file_ptr,  "%s) ) , Xn" ,phase_name) ; 
fprintf  (standalone_out_file_ptr,  "XtXtXt  asserta (current_phase (") ; 
fprintf  (standalone_out_file_ptr,  "%s) ) . XnXn" , nextphase) ; 


} 

void  fail_phase  () 

{ 


fprintf  {out_file_ptr,  "next_phase(%s) Xn",phase_name) ; 

fprintf  (out_file_ptr,  "XtXt  abort (%s) , Xn",phase_naine) ; 

fprintf  (out_file_ptr,  "XtXtXt  retract (current_phase{") ; 

fprintf  (out_file_ptr,  "%s) ) , Xn",phase_name) ; 

fprintf  (out_file_ptr,  "XtXtXt  asserta (current_phase (") ; 

fprintf  (out_file_ptr,  "%s) ) . XnXnXn", failphase) ; 


fprintf  (standalone_out_file_ptr,  "next_phase (%s) Xn",phase_name) ; 
fprintf  (standalone_out_file_ptr, 

"XtXt  abort (%s) , Xn",phase_name) ; 

fprintf  (standalone_out_f ile_ptr,  "XtXtXt  retract (current_phase(") ; 

fprintf  (standalone_out_file_ptr,  "%s) ) ,Xn",phase_naine) ; 
fprintf  (standalone_out_file_ptr,  "XtXtXt  asserta (current_phase (") ; 
fprintf  (standalone_out_file_ptr,  "%s) ). XnXnXn" , failphase) ; 


} 
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/* 

Program: 

Authors : 

Revised: 

System: 

Compiler: 

Compilation: 

Invocation: 

[  input  file: 
[output  file: 


AUV  strategic  level  program 
Brad  Leonhardt  Duane  Davis 
14  May  96 

SUN  Voyager  Solaris  2.4  OS;  SGI  Irix  5.3 
Sun  C;  IRIX  cc 

cc  mission_cpp.c  -o  mission_cpp 

mission_cpp  [normally  invoked  automatically  by  expert  system] 

coinmand_s  t  rings  ] 
mission_graph . C ] 


This  code  is  used  to  create  Prolog  code  to  run  the  Phoenix  Autonomous  Vehicle 
It  can  be  run  with  the  mission_€xpert  prolog  expert  system  which  creates  a  data 
file  for  use  in  the  mission  generator. 


#include  <string.h> 
#include  <stdio.h> 

# include  <ctype.h> 


#define  DATA_FILE  " commandos t rings"  /*  Expert  System  Output  */ 

/*  function  prototypes  */ 


int 

main 

0  ; 

void 

depth_change 

0  ; 

void 

hoverpoint 

0; 

void 

waypoint 

0; 

void 

sonar_search 

0  ; 

void 

get_gps_f ix 

0; 

void 

rotate_sonar_search  { )  ; 

void 

course 

0  ; 

void 

wait 

0  ; 

void 

tube_recovery 

0; 

void 

parse_command 

{); 

void 

successors 

0; 

FILE  *  filejtr; 

FILE  *  out_file_ptr ; 


char  command  [200]; 
char  out_file_name  [200]; 
char  sh€ll_cmd  [200]; 
char  phase_name  [200]; 
char  nextphase  [200]; 
char  failphase  [200]; 
char  cmd  [200]; 


int  variablel , variable2 , variables , variable4 , variables ; 
int  i; 
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/******★***************★********************************* *******************^ 
/*  Parse  command  breaks  input  string  into  variables  to  be  used  in  program  */ 

void  parse_command  (cmd) 
char  *  cmd; 

{ 

sscanf(cmd,  "%s  %s  %s  %s  %d  %d  %d  %d  %d", 

command, phase_name , nextphase , f ai Iphase , 

&variablel ,  &variable2 ,  ScvariableS ,  &variable4 ,  ^variables )  ; 

/*  printf  (''COMMAND  %s  PHASE  NAME  %s  VARl  %d  VAR2  %d  VAR3  %d  VAR4  %d  VARS  %d", 
command, phase_naine,  nextphase,  failphase, 
variablel , variable2 , variables , variable4 , variables ) ; 

*/ 

} 

/**********************★******************************************* ^**^**^*^^ 


int  main  () 

{ 

char  cmd  [200];  /*  input  line  string  of  characters  */ 

/*  Open  file  generated  from  expert  system  */ 

if  ((file^ptr  =  fopen  {DATA_FILE,  "r"])  ==  ({FILE  *)  0)) 

{ 

printf  ("input_read_path:  file  open  failurei \n" ) ; 
return; 

} 

fgets  (cmd,200,file_ptr) ;  /*  Read  a  line  of  data  */ 

parse^command  (cmd)  ; 

/*Create  an  output  file  for  mission  and  copy  mission  controller  data  */ 
if  (strcmp  (command, "file^name" )  ==  0) 

{ 

strcpy  (out__file_name,phase_name)  ; 

strcat (shell_cmd, "cp  controller_cpp. script  "); 

strcat  (shell_cmd,out_file__namG)  ; 

printf ("Shell  command  :%s\n" , shell_cmd) ; 

system  (shell^cmd) ; 

} 

printf ("Output  file  createdXn"); 

if  { (out_file_ptr  =  fopen  (out_f ile^name,  "a"))  ==  ((FILE  *)  0)) 

printf ("INVALID  OUTPUT  FILE  NAME  %s\n" , out_f ile^name) ; 
fclose  (file^tr); 
return (0) ; 

} 


while  ({fgets  (cmd, 200,  filejtr)  l=:NULL))  /*  read  next  line  */ 

{ 

printf ("Input  file  string  %s\n",cmd); 
parse_command(cmd) ; 

/*  Determine  which  template  to  use  */ 

if  (strcmp  (command, "dept h_change" )  ==  0) 
depth_change  ( ) ; 

else  if  (strcmp  (command, "hove rpoint" )  ==  0) 
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hoverpoint  ( ) ; 

else  if  {strcinp  (cominand,  "waypoint'' )  ==  0) 
waypoint  {); 

else  if  (strcinp  {command,  "sonar_search" )  ==  0) 
sona research  (); 

else  if  (strcmp  (command,  "get_,gps_fix" )  ==  0) 
9et_gps_fix  (); 

else  if  (strcmp  (command, "rota te_sonar_search" )  ==  0) 
rotate_sonar_search  ( ) ; 

else  if  (strcmp  (command, "course" )  ==  0) 
course  ( ) ; 

else  if  (strcmp  (command, "wait")  ==  0) 
wait  0; 

else  if  (strcmp  (command, "tube__re cove ry" )  ==  0) 
tube^recovery  ( )  ; 


else 

{ 

printf ("INVALID  PHASE  INPUT  %s\n" , command) ; 

fprintf  (file_ptr,  "\n") ; 

fclose  (file_ptr); 

fprintf  (out_f ile_ptr,  "\n") ; 

fclose  (out_f ile__ptr)  ; 

return  (0) ; 

} 


} 

fprintf  (file^ptr,  "\n"); 
fclose  (file_ptr); 


/*  Pass  2  through  data  file  generated  by  expert  system  */ 

fprintf  (out_file_ptr,  "\n\n\t/*  Link  Phases  Into  DFA  Graph  */\n"); 

if  {(file_p.tr  =  fopen  (DATA_FILE,  "r"))  ==  {{FILE  *)  0)) 

{ 

printf  ("input_read_path:  file  open  failure! \n" ) ; 
return; 

} 

fgets  (cmd,200,file_ptr);  /*  Read  a  line  of  data  */ 

parse_command  (cmd) ; 

if  (strcmp  (command, "file_jiame" )  ==  0) 

{ 

fprintf  (out_f ile_ptr, 

"\tphinitialize->specifySuccessors (ph%s,phmission_abort) ; \n", 
nextphase) ; 

} 

while  {(fgets  (cmd, 200,  file_ptr)  !=:NULL))  /*  read  next  line  */ 

{ 

parse_command  (cmd) ; 
successors  (); 

} 


fprintf  (out_file_ptr,  "\n\treturn  (phini tialize) ; \n\n" ) ; 
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fprintf  (out_f ilejptr,  "}  //  buildMissionGraphXn" ) ; 


fprintf  (file_ptr,  "\n"); 
fclose  (file_ptr); 
fprintf  (out_f ilejtT;  "\n"); 
fclose  (out_filejptr)  ; 


} 


/*********★******************★********************************★*************/ 
/*  Functions  that  produce  the  C++  code  for  the  various  types  of  phases  */ 

/******************************************^f***************************^***.*^ 

void  depth_change  () 

{ 

fprintf  (out_file_i)tr,  "\tDepthChange\t*ph%s  =  phase_name) ; 

fprintf  {out_file_ptr,  "new  DepthChange  (%d,%d) ; \n" , variable2 .  variablel); 

} 


void  hoverpoint  () 

{ 

fprintf  (out_file_ptr,  "\tHover\t\t*ph%s  =  ",phase_name) ; 
fprintf  (out_file_j)tr,  "new  Hover  (%d, %d, %d, %d, %d) ; \n" , 

variable2 , variables , variable4 , variables , variablel ) ; 


} 


void  waypoint  () 
{ 


fprintf  {out_file_ptr,  "\tTransit\t\t*ph%s  =  ",phase_naine) ; 
fprintf  (out_f ile^ptr,  "new  Transit  (%d, %d, %d, %d) ; \n" , 
variables , variables , variable4 , variablel ) ; 


} 


void  sonar_search  () 

{ 


fprintf  (out_file_jptr,  "\ tRotateSearchX t*ph%s  =  " ,phase_name} ; 
fprintf  (out_file_ptr,  "new  RotateSearch  (%d, %d, %d, %d, %d) ; Xn" , 
variables, variables , variable4, variables, variablel) ; 


} 


void  get_gps_fix  {) 

{ 


fprintf  {out_file_jDtr,  "XtGPSFixXtXt*ph%s  =  " ,phase_naine)  ; 
fprintf  (out_file_ptr,  "new  GPSFix  (%d) ; Xn" , variablel) ; 


void  rotate_sonar_search  () 
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{ 


fprintf  (out_filejtr,  "\tSonarSearch\t*ph%s  =  " ,phase_nanie)  ; 
fprintf  {out_file_ptr,  "new  SonarSearch  (%d, %d, %d, %d, %d) ; \n" . 

variable2 , variables , variable4 , variables , variablel ) ; 


void  course  ( ) 

{ 

fprintf  (out_file_ptr,  "\tCourseChange\t*ph%s  =  ",phase_naine) ; 
fprintf  (out_file_ptr, 

"new  CourseChange  (%d, %d) ; \n", variable2,  variablel); 


) 


void  wait  () 
{ 


fprintf  (out_file_ptr,  "\tWait\t\t*ph%s  =  " ,phase_name) ; 
fprintf  (out_file^tr,  "new  Wait  (%d)  ;  \n"  ,  variablel); 


} 


void  tube_recovery  { ) 
{ 


fprintf  (out_file_ptr,  "\tTubeRecovery\ t*ph%s  =  " ,phase__name} ; 
fprintf  (out_file_ptr,  "new  Tub eReco very  (%d, %d, %d, %d, %d) ; \n" . 

variable2 , variables , variable4 , variables , variablel+25  0 ) 


} 


void  successors  () 

{ 


fprintf  (out_file_ptr,  "\tph%s->specifySuccGssors (ph%s, ph%s) ; \n 
phase_name,nextphase, failphase) ; 


} 
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